<commit_before>#ifndef VEXCL_SPMAT_INLINE_SPMV_HPP
#define VEXCL_SPMAT_INLINE_SPMV_HPP

/*
The MIT License

Copyright (c) 2012-2013 Denis Demidov <ddemidov@ksu.ru>

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

/**
 * \file   vexcl/spmat/inline_spmv.hpp
 * \author Denis Demidov <ddemidov@ksu.ru>
 * \brief  Inline SpMV operation.
 */

#include <vexcl/spmat.hpp>

namespace vex {

/// \cond INTERNAL
struct inline_spmv_terminal {};
struct mv_inline_spmv_terminal {};

typedef vector_expression<
    typename boost::proto::terminal< inline_spmv_terminal >::type
    > inline_spmv_terminal_expression;

template <typename val_t, typename col_t, typename idx_t>
struct inline_spmv : inline_spmv_terminal_expression {
    typedef spmv<val_t, col_t, idx_t> Base;
    const typename Base::mat &A;
    const typename Base::vec &x;

    inline_spmv(const Base &base) : A(base.A), x(base.x) {}
};

typedef multivector_expression<
    typename boost::proto::terminal< mv_inline_spmv_terminal >::type
    > mv_inline_spmv_terminal_expression;

template <typename val_t, typename col_t, typename idx_t, class MV>
struct mv_inline_spmv : mv_inline_spmv_terminal_expression {
    typedef multispmv<val_t, col_t, idx_t, MV> Base;
    const typename Base::mat &A;
    const MV                 &x;

    mv_inline_spmv(const Base &base) : A(base.A), x(base.x) {}
};
/// \endcond

template <typename val_t, typename col_t, typename idx_t>
inline_spmv<val_t, col_t, idx_t>
make_inline(const spmv<val_t, col_t, idx_t> &base) {
    precondition(base.x.nparts() == 1, "Can not inline multi-device SpMV operation.");

    return inline_spmv<val_t, col_t, idx_t>(base);
}

template <typename val_t, typename col_t, typename idx_t, class MV>
mv_inline_spmv<val_t, col_t, idx_t, MV>
make_inline(const multispmv<val_t, col_t, idx_t, MV> &base) {
    precondition(base.x(0).nparts() == 1, "Can not inline multi-device SpMV operation.");

    return mv_inline_spmv<val_t, col_t, idx_t, MV>(base);
}

/// \cond INTERNAL
// Allow inline products to participate in vector expressions:
template <>
struct is_vector_expr_terminal< inline_spmv_terminal >
    : std::true_type
{ };

template <>
struct is_multivector_expr_terminal< mv_inline_spmv_terminal >
    : std::true_type
{ };

template <>
struct proto_terminal_is_value< mv_inline_spmv_terminal >
    : std::true_type
{ };

template <size_t I, typename val_t, typename col_t, typename idx_t, typename MV>
struct component< I, mv_inline_spmv<val_t, col_t, idx_t, MV> > {
    typedef inline_spmv<val_t, col_t, idx_t> type;
};

template <size_t I, typename val_t, typename col_t, typename idx_t, typename MV>
inline_spmv<val_t, col_t, idx_t>
get(const mv_inline_spmv<val_t, col_t, idx_t, MV> &t) {
    return make_inline(t.A * t.x(I));
}

template <typename val_t, typename col_t, typename idx_t>
struct kernel_name< inline_spmv<val_t, col_t, idx_t> > {
    static std::string get() {
        return "spmv_";
    }
};

template <typename val_t, typename col_t, typename idx_t>
struct partial_vector_expr< inline_spmv<val_t, col_t, idx_t> > {
    static std::string get(const cl::Device &device, int component, int position, kernel_generator_state &state) {
        return SpMat<val_t, col_t, idx_t>::inline_expression(device, component, position, state);
    }
};

template <typename val_t, typename col_t, typename idx_t>
struct terminal_preamble< inline_spmv<val_t, col_t, idx_t> > {
    static std::string get(const cl::Device &device, int component, int position, kernel_generator_state &state) {
        return SpMat<val_t, col_t, idx_t>::inline_preamble(device, component, position, state);
    }
};

template <typename val_t, typename col_t, typename idx_t>
struct kernel_param_declaration< inline_spmv<val_t, col_t, idx_t> > {
    static std::string get(const cl::Device &device, int component, int position, kernel_generator_state &state) {
        return SpMat<val_t, col_t, idx_t>::inline_parameters(device, component, position, state);
    }
};

template <typename val_t, typename col_t, typename idx_t>
struct kernel_arg_setter< inline_spmv<val_t, col_t, idx_t> > {
    static void set(cl::Kernel &kernel, uint device, size_t index_offset,
            uint &position, const inline_spmv<val_t, col_t, idx_t> &term, kernel_generator_state &state)
    {
        SpMat<val_t, col_t, idx_t>::inline_arguments(
                kernel, device, index_offset, position,
                term.A, term.x, state
                );
    }
};

template <typename val_t, typename col_t, typename idx_t>
struct expression_properties< inline_spmv<val_t, col_t, idx_t> > {
    static void get(const inline_spmv<val_t, col_t, idx_t> &term,
            std::vector<cl::CommandQueue> &queue_list,
            std::vector<size_t> &partition,
            size_t &size
            )
    {
        expression_properties< vector<val_t> >::get(term.x, queue_list, partition, size);
    }
};

/// \endcond

} // namespace vex

#endif
<commit_msg>Documenting make_inline() functions<commit_after>#ifndef VEXCL_SPMAT_INLINE_SPMV_HPP
#define VEXCL_SPMAT_INLINE_SPMV_HPP

/*
The MIT License

Copyright (c) 2012-2013 Denis Demidov <ddemidov@ksu.ru>

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

/**
 * \file   vexcl/spmat/inline_spmv.hpp
 * \author Denis Demidov <ddemidov@ksu.ru>
 * \brief  Inline SpMV operation.
 */

#include <vexcl/spmat.hpp>

namespace vex {

/// \cond INTERNAL
struct inline_spmv_terminal {};
struct mv_inline_spmv_terminal {};

typedef vector_expression<
    typename boost::proto::terminal< inline_spmv_terminal >::type
    > inline_spmv_terminal_expression;

template <typename val_t, typename col_t, typename idx_t>
struct inline_spmv : inline_spmv_terminal_expression {
    typedef spmv<val_t, col_t, idx_t> Base;
    const typename Base::mat &A;
    const typename Base::vec &x;

    inline_spmv(const Base &base) : A(base.A), x(base.x) {}
};

typedef multivector_expression<
    typename boost::proto::terminal< mv_inline_spmv_terminal >::type
    > mv_inline_spmv_terminal_expression;

template <typename val_t, typename col_t, typename idx_t, class MV>
struct mv_inline_spmv : mv_inline_spmv_terminal_expression {
    typedef multispmv<val_t, col_t, idx_t, MV> Base;
    const typename Base::mat &A;
    const MV                 &x;

    mv_inline_spmv(const Base &base) : A(base.A), x(base.x) {}
};
/// \endcond

/// Inlines a sparse matrix - vector product.
/**
 * When applied to a matrix-vector product, the product becomes inlineable.
 * That is, it may be used in any vector expression (not just additive
 * expression). This is only possible in single-device contexts, so user has to
 * guarantee that.
 *
 * Example:
 * \code
 * // Get maximum residual value:
 * eps = sum( fabs(f - vex::make_inline(A * x)) );
 * \endcode
 */
template <typename val_t, typename col_t, typename idx_t>
inline_spmv<val_t, col_t, idx_t>
make_inline(const spmv<val_t, col_t, idx_t> &base) {
    precondition(base.x.nparts() == 1, "Can not inline multi-device SpMV operation.");

    return inline_spmv<val_t, col_t, idx_t>(base);
}

/// Inlines a sparse matrix - multivector product.
/**
 * When applied to a matrix-multivector product, the product becomes
 * inlineable.  That is, it may be used in any multivector expression (not just
 * additive expression). This is only possible in single-device contexts, so
 * user has to guarantee that.
 *
 * Example:
 * \code
 * // Get maximum residual value:
 * eps = sum( fabs(f - vex::make_inline(A * x)) );
 * \endcode
 */
template <typename val_t, typename col_t, typename idx_t, class MV>
mv_inline_spmv<val_t, col_t, idx_t, MV>
make_inline(const multispmv<val_t, col_t, idx_t, MV> &base) {
    precondition(base.x(0).nparts() == 1, "Can not inline multi-device SpMV operation.");

    return mv_inline_spmv<val_t, col_t, idx_t, MV>(base);
}

/// \cond INTERNAL
// Allow inline products to participate in vector expressions:
template <>
struct is_vector_expr_terminal< inline_spmv_terminal >
    : std::true_type
{ };

template <>
struct is_multivector_expr_terminal< mv_inline_spmv_terminal >
    : std::true_type
{ };

template <>
struct proto_terminal_is_value< mv_inline_spmv_terminal >
    : std::true_type
{ };

template <size_t I, typename val_t, typename col_t, typename idx_t, typename MV>
struct component< I, mv_inline_spmv<val_t, col_t, idx_t, MV> > {
    typedef inline_spmv<val_t, col_t, idx_t> type;
};

template <size_t I, typename val_t, typename col_t, typename idx_t, typename MV>
inline_spmv<val_t, col_t, idx_t>
get(const mv_inline_spmv<val_t, col_t, idx_t, MV> &t) {
    return make_inline(t.A * t.x(I));
}

template <typename val_t, typename col_t, typename idx_t>
struct kernel_name< inline_spmv<val_t, col_t, idx_t> > {
    static std::string get() {
        return "spmv_";
    }
};

template <typename val_t, typename col_t, typename idx_t>
struct partial_vector_expr< inline_spmv<val_t, col_t, idx_t> > {
    static std::string get(const cl::Device &device, int component, int position, kernel_generator_state &state) {
        return SpMat<val_t, col_t, idx_t>::inline_expression(device, component, position, state);
    }
};

template <typename val_t, typename col_t, typename idx_t>
struct terminal_preamble< inline_spmv<val_t, col_t, idx_t> > {
    static std::string get(const cl::Device &device, int component, int position, kernel_generator_state &state) {
        return SpMat<val_t, col_t, idx_t>::inline_preamble(device, component, position, state);
    }
};

template <typename val_t, typename col_t, typename idx_t>
struct kernel_param_declaration< inline_spmv<val_t, col_t, idx_t> > {
    static std::string get(const cl::Device &device, int component, int position, kernel_generator_state &state) {
        return SpMat<val_t, col_t, idx_t>::inline_parameters(device, component, position, state);
    }
};

template <typename val_t, typename col_t, typename idx_t>
struct kernel_arg_setter< inline_spmv<val_t, col_t, idx_t> > {
    static void set(cl::Kernel &kernel, uint device, size_t index_offset,
            uint &position, const inline_spmv<val_t, col_t, idx_t> &term, kernel_generator_state &state)
    {
        SpMat<val_t, col_t, idx_t>::inline_arguments(
                kernel, device, index_offset, position,
                term.A, term.x, state
                );
    }
};

template <typename val_t, typename col_t, typename idx_t>
struct expression_properties< inline_spmv<val_t, col_t, idx_t> > {
    static void get(const inline_spmv<val_t, col_t, idx_t> &term,
            std::vector<cl::CommandQueue> &queue_list,
            std::vector<size_t> &partition,
            size_t &size
            )
    {
        expression_properties< vector<val_t> >::get(term.x, queue_list, partition, size);
    }
};

/// \endcond

} // namespace vex

#endif
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: macros.hxx,v $
 *
 *  $Revision: 1.15 $
 *
 *  last change: $Author: hr $ $Date: 2003-04-28 16:26:28 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/
#ifndef _CPPU_MACROS_HXX_
#define _CPPU_MACROS_HXX_

#ifndef _SAL_TYPES_H_
#include <sal/types.h>
#endif
#ifndef _UNO_LBNAMES_H_
#include <uno/lbnames.h>
#endif

/** Namespace name for compiler/ platform, e.g. gcc3, msci */
#define CPPU_CURRENT_NAMESPACE CPPU_ENV

/** Patching the gcc 3 incomatible alignment change for linux.
    This pragma macro is appended by the cppumaker tool to every first member of a struct, iff
    the struct inherits from a base struct the first member is no double or [unsigned] long long.
    @internal
*/
#if defined(__GNUC__) && (defined(LINUX) || defined(FREEBSD)) && (defined(INTEL) || defined(POWERPC) || defined(X86_64) || defined(S390) || defined(SPARC)) && (__GNUC__ == 3)
#define CPPU_GCC3_ALIGN( base_struct ) __attribute__ ((aligned (__alignof__ (base_struct))))
#else
#define CPPU_GCC3_ALIGN( base_struct )
#endif

#endif  // _CPPU_MACROS_HXX_

<commit_msg>INTEGRATION: CWS ooo11rc2 (1.15.14); FILE MERGED 2003/07/14 14:29:55 fa 1.15.14.1: Generalize the CPPU_GCC3_ALIGN define for all gcc 3 platforms instead of explicitly specifying all of them.<commit_after>/*************************************************************************
 *
 *  $RCSfile: macros.hxx,v $
 *
 *  $Revision: 1.16 $
 *
 *  last change: $Author: hr $ $Date: 2003-07-16 17:38:25 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/
#ifndef _CPPU_MACROS_HXX_
#define _CPPU_MACROS_HXX_

#ifndef _SAL_TYPES_H_
#include <sal/types.h>
#endif
#ifndef _UNO_LBNAMES_H_
#include <uno/lbnames.h>
#endif

/** Namespace name for compiler/ platform, e.g. gcc3, msci */
#define CPPU_CURRENT_NAMESPACE CPPU_ENV

/** Patching the gcc 3 incomatible alignment change for linux.
    This pragma macro is appended by the cppumaker tool to every first member of a struct, iff
    the struct inherits from a base struct the first member is no double or [unsigned] long long.
    @internal
*/
#if defined(__GNUC__) && (__GNUC__ >= 3)
#define CPPU_GCC3_ALIGN( base_struct ) __attribute__ ((aligned (__alignof__ (base_struct))))
#else
#define CPPU_GCC3_ALIGN( base_struct )
#endif

#endif  // _CPPU_MACROS_HXX_

<|endoftext|>


<commit_before>// fidi_request_handler.cc ---  -*- mode: c++; -*-

// Copyright 2018-2019 Google LLC
//
// 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
//
// https://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.

/// \file
/// \ingroup app
///
/// This file provides the implementation of the request handling
/// functionality for the fidi (φίδι) HTTP server. This creates
/// multiple instances of the fidi::AppCaller class to actually make
/// downstream calls. This sets up a priority queue, a threadpool, and
/// a task manager to handle calls in parallel and in sequence.

// Code:

#include "src/fidi_request_handler.h"

void
fidi::FidiRequestHandler::handleRequest(Poco::Net::HTTPServerRequest & req,
                                        Poco::Net::HTTPServerResponse &resp) {
  std::ostream &response_stream = resp.send();
  Poco::Logger::get("ConsoleLogger")
      .information("Request from " + req.clientAddress().toString());
  bool failed = false;
  resp.setChunkedTransferEncoding(true);
  resp.setContentType("text/html");

  response_stream << "<title>Fidi  (φίδι) -- a service mock "
                     "instance\n</title>\n"
                     "\n"
                     "
Hello world!
\n"
                     "
Count: "
                  << ++count_
                  << "
\n"
                     "
Method: "
                  << req.getMethod()
                  << "
\n"
                     "
URI: "
                  << req.getURI() << "
\n";
  try {
    driver_.Parse(req.stream());
  } catch (std::bad_alloc &ba) {
    std::cerr << "Got memory error: " << ba.what() << "\n";
    std::cerr.flush();
    return;
  }  // Fail fast on OOM

  auto parse_errors = driver_.get_errors();
  if (parse_errors.first != 0) {
    // take action to return errors
    // Set response, generate error message
    resp.setStatus(Poco::Net::HTTPResponse::HTTP_BAD_REQUEST);
    response_stream << "
Parse Syntax Errors
\n\n\n"
                    << parse_errors.second;
    Poco::Logger::get("ConsoleLogger").error("Errors " + parse_errors.second);
    Poco::Logger::get("FileLogger").error("Errors " + parse_errors.second);
    failed = true;
  }
  std::string warning_message;
  int         warning = driver_.SanityChecks(&warning_message);
  if (warning) {
    resp.setStatus(Poco::Net::HTTPResponse::HTTP_BAD_REQUEST);
    response_stream << "
Warning
\n\n\n" << warning_message;
    Poco::Logger::get("ConsoleLogger").warning("Warnings " + warning_message);
    Poco::Logger::get("FileLogger").warning("Warnings " + warning_message);
    failed = true;
  }
  if (!failed) {
    driver_.set_resp(resp);
    try {
      Poco::Logger::get("ConsoleLogger").trace("Request parsed OK.");
      driver_.Execute(response_stream);
    } catch (std::bad_alloc &ba) {
      std::cerr << "Got memory error: " << ba.what() << "\n";
      std::cerr.flush();
      return;
    }  // Fail fast on OOM
  }
  response_stream << "";
  response_stream.flush();

  Poco::Logger::get("FileLogger")
      .trace("Response sent for count=" + std::to_string(count_) +
             " and URI=" + req.getURI() + "\n");
}

//
// fidi_request_handler.cc ends here
<commit_msg>[fidi]: Implement a /healthz mechanism<commit_after>// fidi_request_handler.cc ---  -*- mode: c++; -*-

// Copyright 2018-2019 Google LLC
//
// 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
//
// https://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.

/// \file
/// \ingroup app
///
/// This file provides the implementation of the request handling
/// functionality for the fidi (φίδι) HTTP server. This creates
/// multiple instances of the fidi::AppCaller class to actually make
/// downstream calls. This sets up a priority queue, a threadpool, and
/// a task manager to handle calls in parallel and in sequence.

// Code:

#include "src/fidi_request_handler.h"

void
fidi::FidiRequestHandler::handleRequest(Poco::Net::HTTPServerRequest & req,
                                        Poco::Net::HTTPServerResponse &resp) {
  std::ostream &response_stream = resp.send();
  Poco::Logger::get("ConsoleLogger")
      .information("Request from " + req.clientAddress().toString());
  bool failed = false;
  resp.setChunkedTransferEncoding(true);
  resp.setContentType("text/html");
  Poco::URI uri(req.getURI());

  if (uri.getPath().compare("/healthz") == 0) {
    Poco::Logger::get("FileLogger").trace("Healthz");
    // TODO: Check for and set a not OK status if we are not healthy
    resp.setStatus(Poco::Net::HTTPResponse::HTTP_OK);
    return;
  }
  response_stream << "<title>Fidi  (φίδι) -- a service mock "
                     "instance\n</title>\n"
                     "\n"
                     "
Hello world!
\n"
                     "
Count: "
                  << ++count_
                  << "
\n"
                     "
Method: "
                  << req.getMethod()
                  << "
\n"
                     "
URI: "
                  << req.getURI() << "
\n";
  try {
    driver_.Parse(req.stream());
  } catch (std::bad_alloc &ba) {
    std::cerr << "Got memory error: " << ba.what() << "\n";
    std::cerr.flush();
    return;
  }  // Fail fast on OOM

  auto parse_errors = driver_.get_errors();
  if (parse_errors.first != 0) {
    // take action to return errors
    // Set response, generate error message
    resp.setStatus(Poco::Net::HTTPResponse::HTTP_BAD_REQUEST);
    response_stream << "
Parse Syntax Errors
\n\n\n"
                    << parse_errors.second;
    Poco::Logger::get("ConsoleLogger").error("Errors " + parse_errors.second);
    Poco::Logger::get("FileLogger").error("Errors " + parse_errors.second);
    failed = true;
  }
  std::string warning_message;
  int         warning = driver_.SanityChecks(&warning_message);
  if (warning) {
    resp.setStatus(Poco::Net::HTTPResponse::HTTP_BAD_REQUEST);
    response_stream << "
Warning
\n\n\n" << warning_message;
    Poco::Logger::get("ConsoleLogger").warning("Warnings " + warning_message);
    Poco::Logger::get("FileLogger").warning("Warnings " + warning_message);
    failed = true;
  }
  if (!failed) {
    driver_.set_resp(resp);
    try {
      Poco::Logger::get("ConsoleLogger").trace("Request parsed OK.");
      driver_.Execute(response_stream);
    } catch (std::bad_alloc &ba) {
      std::cerr << "Got memory error: " << ba.what() << "\n";
      std::cerr.flush();
      return;
    }  // Fail fast on OOM
  }
  response_stream << "";
  response_stream.flush();

  Poco::Logger::get("FileLogger")
      .trace("Response sent for count=" + std::to_string(count_) +
             " and URI=" + req.getURI() + "\n");
}

//
// fidi_request_handler.cc ends here
<|endoftext|>


<commit_before>// Virvo - Virtual Reality Volume Rendering
// Copyright (C) 1999-2003 University of Stuttgart, 2004-2005 Brown University
// Contact: Jurgen P. Schulze, jschulze@ucsd.edu
//
// This file is part of Virvo.
//
// Virvo is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library (see license.txt); if not, write to the
// Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA

#ifdef HAVE_CONFIG_H
#include "vvconfig.h"
#endif

#include "vvdebugmsg.h"
#include "vvinttypes.h"
#include "vvmulticast.h"
#include "vvsocketmonitor.h"
#include "vvinttypes.h"

#include <algorithm>
#include <cmath>

#ifdef HAVE_NORM
#include <normApi.h>
#include <stdlib.h>
#endif

vvMulticast::vvMulticast(const char* addr, const ushort port, const MulticastType type, const MulticastApi api)
: _type(type), _api(api)
{
  if(VV_NORM == _api)
  {
#ifdef HAVE_NORM
    _instance = NormCreateInstance();
    _session = NormCreateSession(_instance, addr, port, NORM_NODE_ANY);

    NormSetCongestionControl(_session, true);
    if(VV_SENDER == type)
    {
      NormSessionId sessionId = (NormSessionId)rand();
      // TODO: Adjust these numbers depending on the used network topology
      NormSetTransmitRate(_session, 8e10);
      NormSetTransmitCacheBounds(_session, CHUNK_SIZE, 1, 128);
      //NormSetGroupSize(_session, 10);
      NormStartSender(_session, sessionId, 1024*1024, 1400, 64, 2);
      NormSetTxSocketBuffer(_session, 1024*1024*32);
    }
    else if(VV_RECEIVER == type)
    {
      NormStartReceiver(_session, 1024*1024);
      NormSetRxSocketBuffer(_session, 1024*1024*64);
      NormDescriptor normDesc = NormGetDescriptor(_instance);
      _nodes.push_back(NormGetLocalNodeId(_session));
      _normSocket = new vvSocket(vvSocket::VV_UDP, int(normDesc));
    }
#else
    (void)addr;
    (void)port;
#endif
  }
  else if(VV_VVSOCKET == _api)
  {
    if(VV_SENDER == _type)
    {
      _socket = new vvSocket(port, addr, vvSocket::VV_MC_SENDER);
    }
    else if(VV_RECEIVER == _type)
    {
      _socket = new vvSocket(port, addr, vvSocket::VV_MC_RECEIVER);
    }
    _socket->init();
  }
}

vvMulticast::~vvMulticast()
{
  if(VV_NORM == _api)
  {
#ifdef HAVE_NORM
    if(VV_SENDER == _type)
    {
      NormStopSender(_session);
    }
    else if(VV_RECEIVER == _type)
    {
      NormStopReceiver(_session);
      delete _normSocket;
    }
    NormDestroySession(_session);
    NormDestroyInstance(_instance);
#endif
  }
  else if(VV_VVSOCKET == _api)
  {
    delete _socket;
  }
}

ssize_t vvMulticast::write(const uchar* bytes, const size_t size, double timeout)
{
  vvDebugMsg::msg(3, "vvMulticast::write()");

  if(VV_NORM == _api)
  {
#ifdef HAVE_NORM
    for(std::vector<NormNodeId>::const_iterator it = _nodes.begin(); it != _nodes.end(); ++it)
    {
      NormAddAckingNode(_session, *it);
    }

    for(unsigned int i=0; i<size; i+=CHUNK_SIZE)
    {
      size_t frameSize = std::min(size_t(CHUNK_SIZE), size);
      _object = NormDataEnqueue(_session, (char*)&bytes[i*CHUNK_SIZE], frameSize);
      NormSetWatermark(_session, _object);

      if(NORM_OBJECT_INVALID ==_object)
      {
        vvDebugMsg::msg(2, "vvMulticast::write(): Norm Object is invalid!");
        return -2;
      }

      NormDescriptor normDesc = NormGetDescriptor(_instance);

      vvSocketMonitor* monitor = new vvSocketMonitor;
      std::vector<vvSocket*> sock;
      sock.push_back(new vvSocket(vvSocket::VV_UDP, int(normDesc)));
      monitor->setReadFds(sock);

      NormEvent theEvent;
      size_t bytesSent = 0;
      bool keepGoing = true;
      while(keepGoing)
      {
        vvSocket* ready = NULL;
        vvSocketMonitor::ErrorType err = monitor->wait(&ready, &timeout);
        if(vvSocketMonitor::VV_TIMEOUT == err)
        {
          vvDebugMsg::msg(2, "vvMulticast::write() timeout reached.");
          return bytesSent;
        }
        else if(vvSocketMonitor::VV_ERROR == err)
        {
          vvDebugMsg::msg(2, "vvMulticast::write() error.");
          return -1;
        }
        else
        {
          NormGetNextEvent(_instance, &theEvent);
          switch(theEvent.type)
          {
          case NORM_CC_ACTIVE:
            vvDebugMsg::msg(3, "vvMulticast::write() NORM_CC_ACTIVE: transmission still active");
            break;
          case NORM_TX_FLUSH_COMPLETED:
          case NORM_LOCAL_SENDER_CLOSED:
          case NORM_TX_OBJECT_SENT:
            vvDebugMsg::msg(3, "vvMulticast::write(): chunk-transfer completed.");
            bytesSent += size_t(NormObjectGetSize(theEvent.object));
            keepGoing = false;
            break;
          default:
            {
              std::string eventmsg = std::string("vvMulticast::write() Norm-Event: ");
              eventmsg += theEvent.type;
              vvDebugMsg::msg(3, eventmsg.c_str());
              break;
            }
          }
        }
      }
    }
    return size;
#else
    (void)bytes;
    (void)size;
    (void)timeout;
    return -1;
#endif
  }
  else
  {
    // number datagrams
    uchar *ndata = numberConsecutively(bytes, size);
    size_t nsize = size+(ceil(float(size)/float((DGRAM_SIZE-4)))*4);

    size_t nleft = nsize;
    while(nleft > 0)
    {
      size_t towrite = std::min(size_t(DGRAM_SIZE), nleft);
      vvSocket::ErrorType err = _socket->write_data((uchar*)&ndata[nsize-nleft], towrite);
      if(vvSocket::VV_OK != err)
      {
        vvDebugMsg::msg(1, "vvMulticast::write() error", true);
        return -1;
      }
      else
      {
        nleft -= towrite;
      }
    }
    return size;
  }
}

ssize_t vvMulticast::read(uchar* data, const size_t size, double timeout)
{
  vvDebugMsg::msg(3, "vvMulticast::read()");

  if(VV_NORM == _api)
  {
#ifdef HAVE_NORM
    vvSocketMonitor monitor;

    std::vector<vvSocket*> sock;
    sock.push_back(_normSocket);
    monitor.setReadFds(sock);

    NormEvent theEvent;
    size_t chunk = 0;
    size_t bytesReceived = 0;
    bool keepGoing = true;
    do
    {
      vvSocket* ready = NULL;
      vvSocketMonitor::ErrorType err = monitor.wait(&ready, &timeout);
      if(vvSocketMonitor::VV_TIMEOUT == err)
      {
        vvDebugMsg::msg(2, "vvMulticast::read() timeout reached.");
        return bytesReceived;
      }
      else if(vvSocketMonitor::VV_ERROR == err)
      {
        vvDebugMsg::msg(2, "vvMulticast::read() error.");
        return -1;
      }
      else
      {
        NormGetNextEvent(_instance, &theEvent);
        switch(theEvent.type)
        {
        case NORM_RX_OBJECT_UPDATED:
          vvDebugMsg::msg(3, "vvMulticast::read() NORM_RX_OBJECT_UPDATED: the identified receive object has newly received data content.");
          break;
        case NORM_RX_OBJECT_COMPLETED:
          {
            vvDebugMsg::msg(3, "vvMulticast::read() NORM_RX_OBJECT_COMPLETED: transfer completed.");
            bytesReceived += size_t(NormObjectGetSize(theEvent.object));
            // copy data into array
            uchar *t_data = (uchar*)NormDataDetachData(theEvent.object);
            for(int i=0;i<NormObjectGetSize(theEvent.object);i++)
            {
              data[i+chunk*CHUNK_SIZE] = t_data[i];
            }
            chunk++;
            break;
          }
        case NORM_RX_OBJECT_ABORTED:
          vvDebugMsg::msg(2, "vvMulticast::read() NORM_RX_OBJECT_ABORTED: transfer incomplete!");
          return -1;
          break;
        default:
          {
            std::string eventmsg = std::string("vvMulticast::read() Norm-Event: ");
            eventmsg += theEvent.type;
            vvDebugMsg::msg(3, eventmsg.c_str());
            break;
          }
        }
      }
      if(bytesReceived >= size) keepGoing = false;
    }
    while((0.0 < timeout || -1.0 == timeout) && keepGoing);
    return bytesReceived;
#else
    (void)size;
    (void)data;
    (void)timeout;
    return -1;
#endif
  }
  else
  {
    size_t nsize = size+(ceil(float(size)/float((DGRAM_SIZE-4)))*4);

    size_t nleft = nsize;
    while(nleft > 0)
    {
      uchar dgram[DGRAM_SIZE];
      dgram[0] = 1;
      dgram[1] = 2;
      dgram[3] = 3;
      dgram[4] = 4;
      dgram[5] = 5;

      ssize_t ret;
      vvSocket::ErrorType err = _socket->read_data(dgram, DGRAM_SIZE, &ret);
      if(vvSocket::VV_OK != err)
      {
        vvDebugMsg::msg(1, "vvMulticast::read() error", true);
        return -1;
      }
      else
      {
        union bytesToInt32
        {
          uchar    x[4];
          uint32_t y;
        };
        bytesToInt32 t;
        t.x[0] = dgram[ret-4];
        t.x[1] = dgram[ret-3];
        t.x[2] = dgram[ret-2];
        t.x[3] = dgram[ret-1];

        uint32_t c = ntohl(t.y);

        size_t pos = c*(DGRAM_SIZE-4);
        for(unsigned int i=0;i<ret-4;i++)
        {
          data[pos+i] = dgram[i];
        }
        nleft -= ret;
      }
    }
    return size;
  }
}

uchar* vvMulticast::numberConsecutively(const uchar* data, const size_t size)
{
  if(VV_NORM == _api)
  {
    vvDebugMsg::msg(1, "vvMulticast::numberConsecutively() is not available (nor necessary) for NormAPI");
    (void) data;
    (void) size;
    return NULL;
  }
  else
  {
    uchar *numbered = new uchar[size_t(size+(ceil(float(size)/float((DGRAM_SIZE-4)))*4))];

    size_t i = 0;
    size_t n = 0;
    uint32_t c = 0;
    while(i < size)
    {
      numbered[n++] = data[i++];
      if((n+4)%DGRAM_SIZE == 0)
      {
        *((uint32_t*)(&numbered[n])) = htonl(c);
        n += 4;
        c++;
      }
    }
    // add last number if dgram not full
    if(n % DGRAM_SIZE != 0)
    {
      *((uint32_t*)(&numbered[n])) = htonl(c);
    }
    return numbered;
  }
}

// vim: sw=2:expandtab:softtabstop=2:ts=2:cino=\:0g0t0
<commit_msg>remove unreachable code<commit_after>// Virvo - Virtual Reality Volume Rendering
// Copyright (C) 1999-2003 University of Stuttgart, 2004-2005 Brown University
// Contact: Jurgen P. Schulze, jschulze@ucsd.edu
//
// This file is part of Virvo.
//
// Virvo is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library (see license.txt); if not, write to the
// Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA

#ifdef HAVE_CONFIG_H
#include "vvconfig.h"
#endif

#include "vvdebugmsg.h"
#include "vvinttypes.h"
#include "vvmulticast.h"
#include "vvsocketmonitor.h"
#include "vvinttypes.h"

#include <algorithm>
#include <cmath>

#ifdef HAVE_NORM
#include <normApi.h>
#include <stdlib.h>
#endif

vvMulticast::vvMulticast(const char* addr, const ushort port, const MulticastType type, const MulticastApi api)
: _type(type), _api(api)
{
  if(VV_NORM == _api)
  {
#ifdef HAVE_NORM
    _instance = NormCreateInstance();
    _session = NormCreateSession(_instance, addr, port, NORM_NODE_ANY);

    NormSetCongestionControl(_session, true);
    if(VV_SENDER == type)
    {
      NormSessionId sessionId = (NormSessionId)rand();
      // TODO: Adjust these numbers depending on the used network topology
      NormSetTransmitRate(_session, 8e10);
      NormSetTransmitCacheBounds(_session, CHUNK_SIZE, 1, 128);
      //NormSetGroupSize(_session, 10);
      NormStartSender(_session, sessionId, 1024*1024, 1400, 64, 2);
      NormSetTxSocketBuffer(_session, 1024*1024*32);
    }
    else if(VV_RECEIVER == type)
    {
      NormStartReceiver(_session, 1024*1024);
      NormSetRxSocketBuffer(_session, 1024*1024*64);
      NormDescriptor normDesc = NormGetDescriptor(_instance);
      _nodes.push_back(NormGetLocalNodeId(_session));
      _normSocket = new vvSocket(vvSocket::VV_UDP, int(normDesc));
    }
#else
    (void)addr;
    (void)port;
#endif
  }
  else if(VV_VVSOCKET == _api)
  {
    if(VV_SENDER == _type)
    {
      _socket = new vvSocket(port, addr, vvSocket::VV_MC_SENDER);
    }
    else if(VV_RECEIVER == _type)
    {
      _socket = new vvSocket(port, addr, vvSocket::VV_MC_RECEIVER);
    }
    _socket->init();
  }
}

vvMulticast::~vvMulticast()
{
  if(VV_NORM == _api)
  {
#ifdef HAVE_NORM
    if(VV_SENDER == _type)
    {
      NormStopSender(_session);
    }
    else if(VV_RECEIVER == _type)
    {
      NormStopReceiver(_session);
      delete _normSocket;
    }
    NormDestroySession(_session);
    NormDestroyInstance(_instance);
#endif
  }
  else if(VV_VVSOCKET == _api)
  {
    delete _socket;
  }
}

ssize_t vvMulticast::write(const uchar* bytes, const size_t size, double timeout)
{
  vvDebugMsg::msg(3, "vvMulticast::write()");

  if(VV_NORM == _api)
  {
#ifdef HAVE_NORM
    for(std::vector<NormNodeId>::const_iterator it = _nodes.begin(); it != _nodes.end(); ++it)
    {
      NormAddAckingNode(_session, *it);
    }

    for(unsigned int i=0; i<size; i+=CHUNK_SIZE)
    {
      size_t frameSize = std::min(size_t(CHUNK_SIZE), size);
      _object = NormDataEnqueue(_session, (char*)&bytes[i*CHUNK_SIZE], frameSize);
      NormSetWatermark(_session, _object);

      if(NORM_OBJECT_INVALID ==_object)
      {
        vvDebugMsg::msg(2, "vvMulticast::write(): Norm Object is invalid!");
        return -2;
      }

      NormDescriptor normDesc = NormGetDescriptor(_instance);

      vvSocketMonitor* monitor = new vvSocketMonitor;
      std::vector<vvSocket*> sock;
      sock.push_back(new vvSocket(vvSocket::VV_UDP, int(normDesc)));
      monitor->setReadFds(sock);

      NormEvent theEvent;
      size_t bytesSent = 0;
      bool keepGoing = true;
      while(keepGoing)
      {
        vvSocket* ready = NULL;
        vvSocketMonitor::ErrorType err = monitor->wait(&ready, &timeout);
        if(vvSocketMonitor::VV_TIMEOUT == err)
        {
          vvDebugMsg::msg(2, "vvMulticast::write() timeout reached.");
          return bytesSent;
        }
        else if(vvSocketMonitor::VV_ERROR == err)
        {
          vvDebugMsg::msg(2, "vvMulticast::write() error.");
          return -1;
        }
        else
        {
          NormGetNextEvent(_instance, &theEvent);
          switch(theEvent.type)
          {
          case NORM_CC_ACTIVE:
            vvDebugMsg::msg(3, "vvMulticast::write() NORM_CC_ACTIVE: transmission still active");
            break;
          case NORM_TX_FLUSH_COMPLETED:
          case NORM_LOCAL_SENDER_CLOSED:
          case NORM_TX_OBJECT_SENT:
            vvDebugMsg::msg(3, "vvMulticast::write(): chunk-transfer completed.");
            bytesSent += size_t(NormObjectGetSize(theEvent.object));
            keepGoing = false;
            break;
          default:
            {
              std::string eventmsg = std::string("vvMulticast::write() Norm-Event: ");
              eventmsg += theEvent.type;
              vvDebugMsg::msg(3, eventmsg.c_str());
              break;
            }
          }
        }
      }
    }
    return size;
#else
    (void)bytes;
    (void)size;
    (void)timeout;
    return -1;
#endif
  }
  else
  {
    // number datagrams
    uchar *ndata = numberConsecutively(bytes, size);
    size_t nsize = size+(ceil(float(size)/float((DGRAM_SIZE-4)))*4);

    size_t nleft = nsize;
    while(nleft > 0)
    {
      size_t towrite = std::min(size_t(DGRAM_SIZE), nleft);
      vvSocket::ErrorType err = _socket->write_data((uchar*)&ndata[nsize-nleft], towrite);
      if(vvSocket::VV_OK != err)
      {
        vvDebugMsg::msg(1, "vvMulticast::write() error", true);
        return -1;
      }
      else
      {
        nleft -= towrite;
      }
    }
    return size;
  }
}

ssize_t vvMulticast::read(uchar* data, const size_t size, double timeout)
{
  vvDebugMsg::msg(3, "vvMulticast::read()");

  if(VV_NORM == _api)
  {
#ifdef HAVE_NORM
    vvSocketMonitor monitor;

    std::vector<vvSocket*> sock;
    sock.push_back(_normSocket);
    monitor.setReadFds(sock);

    NormEvent theEvent;
    size_t chunk = 0;
    size_t bytesReceived = 0;
    bool keepGoing = true;
    do
    {
      vvSocket* ready = NULL;
      vvSocketMonitor::ErrorType err = monitor.wait(&ready, &timeout);
      if(vvSocketMonitor::VV_TIMEOUT == err)
      {
        vvDebugMsg::msg(2, "vvMulticast::read() timeout reached.");
        return bytesReceived;
      }
      else if(vvSocketMonitor::VV_ERROR == err)
      {
        vvDebugMsg::msg(2, "vvMulticast::read() error.");
        return -1;
      }
      else
      {
        NormGetNextEvent(_instance, &theEvent);
        switch(theEvent.type)
        {
        case NORM_RX_OBJECT_UPDATED:
          vvDebugMsg::msg(3, "vvMulticast::read() NORM_RX_OBJECT_UPDATED: the identified receive object has newly received data content.");
          break;
        case NORM_RX_OBJECT_COMPLETED:
          {
            vvDebugMsg::msg(3, "vvMulticast::read() NORM_RX_OBJECT_COMPLETED: transfer completed.");
            bytesReceived += size_t(NormObjectGetSize(theEvent.object));
            // copy data into array
            uchar *t_data = (uchar*)NormDataDetachData(theEvent.object);
            for(int i=0;i<NormObjectGetSize(theEvent.object);i++)
            {
              data[i+chunk*CHUNK_SIZE] = t_data[i];
            }
            chunk++;
            break;
          }
        case NORM_RX_OBJECT_ABORTED:
          vvDebugMsg::msg(2, "vvMulticast::read() NORM_RX_OBJECT_ABORTED: transfer incomplete!");
          return -1;
          break;
        default:
          {
            std::string eventmsg = std::string("vvMulticast::read() Norm-Event: ");
            eventmsg += theEvent.type;
            vvDebugMsg::msg(3, eventmsg.c_str());
            break;
          }
        }
      }
      if(bytesReceived >= size) keepGoing = false;
    }
    while((0.0 < timeout || -1.0 == timeout) && keepGoing);
    return bytesReceived;
#else
    (void)size;
    (void)data;
    (void)timeout;
    return -1;
#endif
  }
  else
  {
    size_t nsize = size+(ceil(float(size)/float((DGRAM_SIZE-4)))*4);

    size_t nleft = nsize;
    while(nleft > 0)
    {
      uchar dgram[DGRAM_SIZE];
      dgram[0] = 1;
      dgram[1] = 2;
      dgram[3] = 3;
      dgram[4] = 4;
      dgram[5] = 5;

      ssize_t ret;
      vvSocket::ErrorType err = _socket->read_data(dgram, DGRAM_SIZE, &ret);
      if(vvSocket::VV_OK != err)
      {
        vvDebugMsg::msg(1, "vvMulticast::read() error", true);
        return -1;
      }
      else
      {
        union bytesToInt32
        {
          uchar    x[4];
          uint32_t y;
        };
        bytesToInt32 t;
        t.x[0] = dgram[ret-4];
        t.x[1] = dgram[ret-3];
        t.x[2] = dgram[ret-2];
        t.x[3] = dgram[ret-1];

        uint32_t c = ntohl(t.y);

        size_t pos = c*(DGRAM_SIZE-4);
        for(unsigned int i=0;i<ret-4;i++)
        {
          data[pos+i] = dgram[i];
        }
        nleft -= ret;
      }
    }
    return size;
  }
}

uchar* vvMulticast::numberConsecutively(const uchar* data, const size_t size)
{
  uchar *numbered = new uchar[size_t(size+(ceil(float(size)/float((DGRAM_SIZE-4)))*4))];

  size_t i = 0;
  size_t n = 0;
  uint32_t c = 0;
  while(i < size)
  {
    numbered[n++] = data[i++];
    if((n+4)%DGRAM_SIZE == 0)
    {
      *((uint32_t*)(&numbered[n])) = htonl(c);
      n += 4;
      c++;
    }
  }
  // add last number if dgram not full
  if(n % DGRAM_SIZE != 0)
  {
    *((uint32_t*)(&numbered[n])) = htonl(c);
  }
  return numbered;
}

// vim: sw=2:expandtab:softtabstop=2:ts=2:cino=\:0g0t0
<|endoftext|>


<commit_before>// Copyright (c) 2016 Tchernopyatov Alexey. Contacts: alexey@tchernopyatov.com
// Under MIT license, view LICENSE.txt

#include "createexercisedialog.h"
#include <QWidget>
#include <QLabel>
#include <QBoxLayout>
#include <QSpinBox>
#include <QPushButton>
#include <QFileDialog>
#include <QApplication>
#include <QPixmap>
#include <QDialog>
#include <QDebug>
#include <QLineEdit>
#include "exercisewidget.h"
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#include <QVariantMap>

CreateExerciseDialog::CreateExerciseDialog(QWidget *parent) : QDialog(parent)
{
    QVBoxLayout* mainLout = new QVBoxLayout(this);
    createBaseSettings(mainLout);
    createExerciseField(mainLout);
    createConfirmButton(mainLout);
    mainLout->addStretch(1);
}

CreateExerciseDialog::~CreateExerciseDialog()
{

}

/**
 * set language for dialog
 * @param lang - words for set
 * @author Чернопятов А.В.
 * @date 2015.02.07
 */
void CreateExerciseDialog::setLanguage(const LanguageStruct &lang) {
    currentLanguage = lang;
    nameLbl->setText(currentLanguage.words["CREATE_EXERCISE_DLG_CYCLE_NAME"]);
    setCountSpinBox->setPrefix(currentLanguage.words["CREATE_EXERCISE_DLG_SET_COUNT"]);
    addExerciseBtn->setText(currentLanguage.words["CREATE_EXERCISE_DLG_ADD_EXERCISE"]);
    this->btnOk->setText(currentLanguage.words["BTN_OK"]);
    this->btnCancel->setText(currentLanguage.words["BTN_CANCEL"]);
    for(int i = 0; i < exersiseVector.size(); i++) {
        exersiseVector[i]->setLanguage(lang);
    }
}

void CreateExerciseDialog::createBaseSettings(QBoxLayout*lout) {
    {
        QHBoxLayout* hlout = new QHBoxLayout();
        nameLbl = new QLabel(this);
        nameLbl->setText(currentLanguage.words["CREATE_EXERCISE_DLG_CYCLE_NAME"]);
        hlout->addWidget(nameLbl);
        nameEdit = new QLineEdit(this);
        hlout->addWidget(nameEdit);
        lout->addLayout(hlout);
    }
    {
        QHBoxLayout* hlout = new QHBoxLayout();
        setCountSpinBox = new QSpinBox(this);
        setCountSpinBox->setPrefix(currentLanguage.words["CREATE_EXERCISE_DLG_SET_COUNT"]);
        setCountSpinBox->setMinimum(1);
        setCountSpinBox->setMaximum(32);
        hlout->addWidget(setCountSpinBox);

        addExerciseBtn = new QPushButton(this);
        addExerciseBtn->setText(currentLanguage.words["CREATE_EXERCISE_DLG_ADD_EXERCISE"]);
        connect(addExerciseBtn,SIGNAL(clicked()),this,SLOT(pushButtonClicked()));
        hlout->addWidget(addExerciseBtn);

        lout->addLayout(hlout);
    }
}

void CreateExerciseDialog::createExerciseField(QBoxLayout *lout) {
    exerciseLout = new QGridLayout();
    lout->addLayout(exerciseLout);

}

ExerciseWidget* CreateExerciseDialog::createExersise() {
    QString title = currentLanguage.words["CREATE_EXERCISE_DLG_EXERCISE"]+ QString::number(exersiseVector.size()+1);
    ExerciseWidget* wdg = new ExerciseWidget(this);
    connect(wdg,SIGNAL(removeMe()),this,SLOT(removeExercise()));
    wdg->setBoxTitle(title);
    exersiseVector.push_back(wdg);
    return wdg;
}

void CreateExerciseDialog::pushButtonClicked() {
    QPushButton* btn =  (qobject_cast<QPushButton*>(sender()));
    if(btn == addExerciseBtn) {
        this->createExersise();
        updateGrid();
    }
}

void CreateExerciseDialog::createConfirmButton(QBoxLayout *lout) {
    btnOk = new QPushButton(currentLanguage.words["BTN_OK"],this);
    btnCancel = new QPushButton(currentLanguage.words["BTN_CANCEL"],this);
    QHBoxLayout *hlout = new QHBoxLayout();
    hlout->addWidget(btnOk);
    hlout->addWidget(btnCancel);
    connect(btnOk,SIGNAL(clicked()),SLOT(accept()));
    connect(btnCancel,SIGNAL(clicked()),SLOT(reject()));

    lout->addLayout(hlout);
}

void CreateExerciseDialog::removeExercise() {
       ExerciseWidget* wdg =  (qobject_cast<ExerciseWidget*>(sender()));
       disconnect(wdg,SIGNAL(removeMe()),this,SLOT(removeExercise()));
       exerciseLout->removeWidget(wdg);
       exersiseVector.removeOne(wdg);
       wdg->deleteLater();

       updateGrid();

}

void CreateExerciseDialog::updateGrid() {
    for(int i = 0; i < exersiseVector.size(); i++) {
         QString title = currentLanguage.words["CREATE_EXERCISE_DLG_EXERCISE"] + QString::number(i+1);
         exersiseVector[i]->setBoxTitle(title);
         exerciseLout->removeWidget(exersiseVector[i]);
         int maxR = 3;
         int maxC = 3;
         int sz = i;
         int row = sz/maxR;
         int col = sz - row*maxC;
         exerciseLout->addWidget(exersiseVector[i],row,col);
    }
}

QByteArray CreateExerciseDialog::getExerciseData() {
    QJsonDocument doc;
    QVariantMap map;
    map.insert("name", nameEdit->text());
    map.insert("set_count",setCountSpinBox->value());
    QVariantMap step;
    QVariantList lst;
    ExerciseStruct es;
    for(int i = 0; i< exersiseVector.size(); i++) {
        es= exersiseVector.at(i)->getData();
        step["name"] = es.name;
        step["time"] = es.time;
        lst.push_back(step);
    }
    map.insert("exercises",lst);

    QJsonObject json = QJsonObject::fromVariantMap(map);

    //obj["name"]=nameEdit->text();
    doc.setObject(json);
    qDebug() << " " << doc.toJson();
    return doc.toJson();
}

void CreateExerciseDialog::loadExerciseData(const QByteArray &data) {
    qDebug() << " data = " << data;
    QJsonDocument doc;
    doc = QJsonDocument::fromJson(data);
    qDebug() << "readed " << doc.toJson();
    SetStruct ss(doc);
    this->loadExerciseData(ss);
    updateGrid();
}

void CreateExerciseDialog::loadExerciseData(const SetStruct &data) {
    nameEdit->setText(data.name);
    setCountSpinBox->setValue(data.count);
    for(int i = 0; i < data.exercise.size(); i++) {
        qDebug() << "[" << i << "]" << data.exercise[i].name << " time = " << data.exercise[i].time;
        ExerciseWidget* wdg = this->createExersise();
        ExerciseStruct es;
        es.name = data.exercise[i].name;
        es.time = data.exercise[i].time;
        wdg->setData(es);
    }
}
<commit_msg>Update createexercisedialog.cpp<commit_after>// Copyright (c) 2016 Tchernopyatov Alexey. Contacts: alexey@tchernopyatov.com
// Under MIT license, view LICENSE.txt

#include "createexercisedialog.h"
#include <QWidget>
#include <QLabel>
#include <QBoxLayout>
#include <QSpinBox>
#include <QPushButton>
#include <QFileDialog>
#include <QApplication>
#include <QPixmap>
#include <QDialog>
#include <QDebug>
#include <QLineEdit>
#include "exercisewidget.h"
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#include <QVariantMap>

CreateExerciseDialog::CreateExerciseDialog(QWidget *parent) : QDialog(parent)
{
    QVBoxLayout* mainLout = new QVBoxLayout(this);
    createBaseSettings(mainLout);
    createExerciseField(mainLout);
    createConfirmButton(mainLout);
    mainLout->addStretch(1);
}

CreateExerciseDialog::~CreateExerciseDialog()
{

}

/**
 * set language for dialog
 * @param lang - words for set
 * @author Чернопятов А.В.
 * @date 2015.02.07
 */
void CreateExerciseDialog::setLanguage(const LanguageStruct &lang) {
    currentLanguage = lang;
    nameLbl->setText(currentLanguage.words["CREATE_EXERCISE_DLG_CYCLE_NAME"]);
    setCountSpinBox->setPrefix(currentLanguage.words["CREATE_EXERCISE_DLG_SET_COUNT"]);
    addExerciseBtn->setText(currentLanguage.words["CREATE_EXERCISE_DLG_ADD_EXERCISE"]);
    this->btnOk->setText(currentLanguage.words["BTN_OK"]);
    this->btnCancel->setText(currentLanguage.words["BTN_CANCEL"]);
    for(int i = 0; i < exersiseVector.size(); i++) {
        exersiseVector[i]->setLanguage(lang);
    }
}

void CreateExerciseDialog::createBaseSettings(QBoxLayout*lout) {
    {
        QHBoxLayout* hlout = new QHBoxLayout();
        nameLbl = new QLabel(this);
        nameLbl->setText(currentLanguage.words["CREATE_EXERCISE_DLG_CYCLE_NAME"]);
        hlout->addWidget(nameLbl);
        nameEdit = new QLineEdit(this);
        hlout->addWidget(nameEdit);
        lout->addLayout(hlout);
    }
    {
        QHBoxLayout* hlout = new QHBoxLayout();
        setCountSpinBox = new QSpinBox(this);
        setCountSpinBox->setPrefix(currentLanguage.words["CREATE_EXERCISE_DLG_SET_COUNT"]);
        setCountSpinBox->setMinimum(1);
        setCountSpinBox->setMaximum(32);
        hlout->addWidget(setCountSpinBox);

        addExerciseBtn = new QPushButton(this);
        addExerciseBtn->setText(currentLanguage.words["CREATE_EXERCISE_DLG_ADD_EXERCISE"]);
        connect(addExerciseBtn,SIGNAL(clicked()),this,SLOT(pushButtonClicked()));
        hlout->addWidget(addExerciseBtn);

        lout->addLayout(hlout);
    }
}

void CreateExerciseDialog::createExerciseField(QBoxLayout *lout) {
    exerciseLout = new QGridLayout();
    lout->addLayout(exerciseLout);

}

ExerciseWidget* CreateExerciseDialog::createExersise() {
    QString title = currentLanguage.words["CREATE_EXERCISE_DLG_EXERCISE"]+ QString::number(exersiseVector.size()+1);
    ExerciseWidget* wdg = new ExerciseWidget(this);
    connect(wdg,SIGNAL(removeMe()),this,SLOT(removeExercise()));
    wdg->setBoxTitle(title);
    exersiseVector.push_back(wdg);
    return wdg;
}

void CreateExerciseDialog::pushButtonClicked() {
    QPushButton* btn =  (qobject_cast<QPushButton*>(sender()));
    if(btn == addExerciseBtn) {
        this->createExersise();
        updateGrid();
    }
}

void CreateExerciseDialog::createConfirmButton(QBoxLayout *lout) {
    btnOk = new QPushButton(currentLanguage.words["BTN_OK"],this);
    btnCancel = new QPushButton(currentLanguage.words["BTN_CANCEL"],this);
    QHBoxLayout *hlout = new QHBoxLayout();
    hlout->addWidget(btnOk);
    hlout->addWidget(btnCancel);
    connect(btnOk,SIGNAL(clicked()),SLOT(accept()));
    connect(btnCancel,SIGNAL(clicked()),SLOT(reject()));

    lout->addLayout(hlout);
}

void CreateExerciseDialog::removeExercise() {
       ExerciseWidget* wdg =  (qobject_cast<ExerciseWidget*>(sender()));
       disconnect(wdg,SIGNAL(removeMe()),this,SLOT(removeExercise()));
       exerciseLout->removeWidget(wdg);
       exersiseVector.removeOne(wdg);
       wdg->deleteLater();

       updateGrid();

}

void CreateExerciseDialog::updateGrid() {
    for(int i = 0; i < exersiseVector.size(); i++) {
         QString title = currentLanguage.words["CREATE_EXERCISE_DLG_EXERCISE"] + QString::number(i+1);
         exersiseVector[i]->setBoxTitle(title);
         exerciseLout->removeWidget(exersiseVector[i]);
         int maxR = 3;
         int maxC = 3;
         int sz = i;
         int row = sz/maxR;
         int col = sz - row*maxC;
         exerciseLout->addWidget(exersiseVector[i],row,col);
    }
}

QByteArray CreateExerciseDialog::getExerciseData() {
    QJsonDocument doc;
    QVariantMap map;
    map.insert("name", nameEdit->text());
    map.insert("set_count",setCountSpinBox->value());
    QVariantMap step;
    QVariantList lst;
    ExerciseStruct es;
    for(int i = 0; i< exersiseVector.size(); i++) {
        es= exersiseVector.at(i)->getData();
        step["name"] = es.name;
        step["time"] = es.time;
        lst.push_back(step);
    }
    map.insert("exercises",lst);

    QJsonObject json = QJsonObject::fromVariantMap(map);

    //obj["name"]=nameEdit->text();
    doc.setObject(json);
    qDebug() << " " << doc.toJson();
    return doc.toJson();
}

void CreateExerciseDialog::loadExerciseData(const QByteArray &data) {
    qDebug() << " data = " << data;
    QJsonDocument doc;
    doc = QJsonDocument::fromJson(data);
    qDebug() << "readed " << doc.toJson();
    SetStruct ss(doc);
    this->loadExerciseData(ss);
    updateGrid();
}

void CreateExerciseDialog::loadExerciseData(const SetStruct &data) {
    nameEdit->setText(data.name);
    setCountSpinBox->setValue(data.count);
    for(int i = 0; i < data.exercise.size(); i++) {
        qDebug() << "[" << i << "]" << data.exercise[i].name << " time = " << data.exercise[i].time;
        ExerciseWidget* wdg = this->createExersise();
        wdg->setLanguage(currentLanguage);
        ExerciseStruct es;
        es.name = data.exercise[i].name;
        es.time = data.exercise[i].time;
        wdg->setData(es);
    }
}
<|endoftext|>


<commit_before>#include "tidyup_state_creators/stateCreatorFromPlanningScene.h"
#include "tidyup_utils/stringutil.h"
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <moveit/move_group/capability_names.h>
#include <moveit_msgs/GetPlanningScene.h>
#include <moveit_msgs/PlanningScene.h>

PLUGINLIB_EXPORT_CLASS(tidyup_state_creators::StateCreatorFromPlanningScene, continual_planning_executive::StateCreator)

namespace tidyup_state_creators
{
    StateCreatorFromPlanningScene::StateCreatorFromPlanningScene()
    {
    	ros::NodeHandle nh;
    	srvPlanningScene_ = nh.serviceClient<moveit_msgs::GetPlanningScene>(move_group::GET_PLANNING_SCENE_SERVICE_NAME);
    }

    StateCreatorFromPlanningScene::~StateCreatorFromPlanningScene()
    {
    }

    void StateCreatorFromPlanningScene::initialize(const std::deque<std::string>& arguments)
    {
    }

    bool StateCreatorFromPlanningScene::fillState(SymbolicState& state)
    {
    	initializePlanningScene();
    	// Tables have been added to symbolic state in goalCreatorLoadTablesIntoPlanningScene
    	initializeTables(state);

    	ROS_DEBUG_STREAM("StateCreatorFromPlanningScene::" << __func__ << ": number of collision objects in planning scene: "
    			<< planningScene_.world.collision_objects.size());

    	forEach(const moveit_msgs::CollisionObject& object, planningScene_.world.collision_objects)
		{
    		ROS_DEBUG_STREAM("StateCreatorFromPlanningScene::" << __func__ << ": processing object: " << object.id);

    		if (StringUtil::startsWith(object.id, "table"))
    		{
    			// tables are already in symbolic state - load in goalCreatorLoadTablesIntoPlanningScene
    			continue;
    		}
            if (StringUtil::startsWith(object.id, "door"))
            {
                continue;
            }
            if (StringUtil::startsWith(object.id, "sponge"))
            {
                continue;
            }
    		addObjectToSymbolicState(state, object, "movable_object");
    		findMatchingTable(state, planningScene_.world.collision_objects, object);
		}
    	// attached objects
    	forEach(const moveit_msgs::AttachedCollisionObject& attachedObject, planningScene_.robot_state.attached_collision_objects)
    	{
    		const moveit_msgs::CollisionObject& object = attachedObject.object;
    		addObjectToSymbolicState(state, object, "movable_object");

            // grasped predicate
            vector<string> params;
            params.push_back(object.id);
            params.push_back("arm_name");
            if (StringUtil::startsWith(attachedObject.link_name, "l_"))
            {
                ROS_DEBUG_STREAM("processing attached object " << object.id << " on left_arm.");
                params[1] = "left_arm";
                state.setBooleanPredicate("object-grasped", params, true);
            }
            else if (StringUtil::startsWith(attachedObject.link_name, "r_"))
            {
                ROS_DEBUG_STREAM("processing attached object " << object.id << " on right_arm.");
                params[1] = "right_arm";
                state.setBooleanPredicate("object-grasped", params, true);
            }
            else
            {
                ROS_ERROR_STREAM("processing attached object " << object.id << " on unknown link.");
            }
    	}

    	return true;
    }

    void StateCreatorFromPlanningScene::initializePlanningScene()
    {
        ROS_INFO("StateCreatorFromPlanningScene::%s: Waiting for %s service.",
        		__func__, move_group::GET_PLANNING_SCENE_SERVICE_NAME.c_str());
        srvPlanningScene_.waitForExistence();

        moveit_msgs::GetPlanningScene::Request request;
        moveit_msgs::GetPlanningScene::Response response;
        //request.components.WORLD_OBJECT_NAMES; IMPORTANT: This declaration does not work!
        request.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY |
            moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS;

        if (!srvPlanningScene_.call(request, response))
        {
            ROS_ERROR("StateCreatorFromPlanningScene::%s: Failed to get initial planning scene.", __func__);
        }

        ROS_DEBUG("StateCreatorFromPlanningScene::%s: Number of collision objects: %lu",
        		__func__, response.scene.world.collision_objects.size());

        setPlanningScene(response.scene);
    }

    void StateCreatorFromPlanningScene::setPlanningScene(const moveit_msgs::PlanningScene& scene)
    {
    	planningScene_ = scene;
    }

    void StateCreatorFromPlanningScene::initializeTables(const SymbolicState& currentState)
    {
        ROS_DEBUG_STREAM("processing tables");
        pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> tablesRange =
                currentState.getTypedObjects().equal_range("table");
        for (SymbolicState::TypedObjectConstIterator tablesIterator = tablesRange.first;
                tablesIterator != tablesRange.second; tablesIterator++)
        {
            ROS_DEBUG_STREAM("processing "<<tablesIterator->second);
            tables_.insert(tablesIterator->second);
//            pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> locationsRange =
//                    currentState.getTypedObjects().equal_range("manipulation_location");
//            for (SymbolicState::TypedObjectConstIterator locationsIterator = locationsRange.first;
//                    locationsIterator != locationsRange.second; locationsIterator++)
//            {
//                // (location-near-table ?l - manipulation-location ?t - table)
//                Predicate pAt;
//                pAt.name = "location-near-table";
//                pAt.parameters.push_back(locationsIterator->second);
//                pAt.parameters.push_back(tablesIterator->second);
//                bool value = false;
//                currentState.hasBooleanPredicate(pAt, &value);
//                if (value)
//                {
//                    ROS_DEBUG_STREAM("adding location "<<locationsIterator->second<<" to "<<tablesIterator->second);
//                    tableLocations_.insert(make_pair(tablesIterator->second, locationsIterator->second));
//                }
//            }
        }
    }

    bool StateCreatorFromPlanningScene::extractPoseStampedFromCollisionObject(const moveit_msgs::CollisionObject& co,
    		geometry_msgs::PoseStamped& pose) const
    {
    	if (co.mesh_poses.empty() && co.primitive_poses.empty())
    	{
    		ROS_WARN("stateCreatorFromPlanningScene::%s: CollisionObject %s had no mesh_poses nor primitive_poses", __func__, co.id.c_str());
    		return false;
    	}

        std::vector<geometry_msgs::Pose> poses;
        if(!co.mesh_poses.empty() && !co.primitive_poses.empty()) {
            ROS_WARN("%s: CollisionObject %s had mesh_poses and primitive_poses -> using primitive_poses",
                    __func__, co.id.c_str());
            poses = co.primitive_poses;
        } else if(!co.primitive_poses.empty()) {
            poses = co.primitive_poses;
        } else {
            ROS_ASSERT(!co.mesh_poses.empty());
            poses = co.mesh_poses;
        }

        if(poses.size() > 1) {
            ROS_WARN("%s: CollisionObject %s had %zu poses -> using first.", __func__, co.id.c_str(), poses.size());
        }
        pose.pose = poses.front();
        pose.header = co.header;
    	return true;
    }

    void StateCreatorFromPlanningScene::addObjectToSymbolicState(SymbolicState& state, const moveit_msgs::CollisionObject& co,
    		const std::string& objectType)
    {
    	// Verify that objectType is spelled correctly
    	if (!doesObjectTypeExist(objectType))
    		return;
    	state.addObject(co.id, objectType);
        state.setNumericalFluent("timestamp", co.id, co.header.stamp.toSec());
        state.addObject(co.header.frame_id, "frameid");
        ROS_DEBUG_STREAM("StateCreatorFromPlanningScene::" << __func__ << " object: " << co.id
        		<< " has frame: " << co.header.frame_id);
        state.setObjectFluent("frame-id", co.id, co.header.frame_id);

        geometry_msgs::PoseStamped poseStamped;
        if (!extractPoseStampedFromCollisionObject(co, poseStamped))
        {
        	ROS_ERROR("StateCreatorFromPlanningScene::%s: object:%s does not have a pose!", __func__, co.id.c_str());
        	return;
        }
        geometry_msgs::Pose pose = poseStamped.pose;
        state.setNumericalFluent("x", co.id, pose.position.x);
        state.setNumericalFluent("y", co.id, pose.position.y);
        state.setNumericalFluent("z", co.id, pose.position.z);
        state.setNumericalFluent("qx", co.id, pose.orientation.x);
        state.setNumericalFluent("qy", co.id, pose.orientation.y);
        state.setNumericalFluent("qz", co.id, pose.orientation.z);
        state.setNumericalFluent("qw", co.id, pose.orientation.w);
    }

    bool StateCreatorFromPlanningScene::doesObjectTypeExist(const string& objectType)
    {
    	std::string types[] = { "pose", "frameid", "location", "manipulation_location",
    							"table", "movable_object", "arm", "arm_state" };
    	std::set<std::string> objectTypes(types, types + sizeof(types) / sizeof(types[0]));
    	ROS_ASSERT(objectTypes.size() == 8);

    	if (objectTypes.find(objectType) != objectTypes.end())
    	{
    		return true;
    	}
    	else
    	{
    		ROS_ERROR("StateCreatorFromPlanningScene::%s: Object Type %s does not exist "
    				"- maybe typo", __func__, objectType.c_str());
    		return false;
    	}
    }

    void StateCreatorFromPlanningScene::findMatchingTable(SymbolicState& currentState,
    		const std::vector<moveit_msgs::CollisionObject>& allCos,
    		const moveit_msgs::CollisionObject& co)
    {
        string closest_table = "table";
        double closest_distance = 2.0;
        forEach(const moveit_msgs::CollisionObject& table, allCos)
        {
        	// if collisionObject table is really a table (was added in initializedTables())
            if (tables_.find(table.id) != tables_.end())
            {
                geometry_msgs::PoseStamped tablePoseStamped;
                if (!extractPoseStampedFromCollisionObject(table, tablePoseStamped))
                {
                	ROS_ERROR("StateCreatorFromPlanningScene::%s: table:%s does not have a pose!", __func__, table.id.c_str());
                	return;
                }
                const geometry_msgs::Point& origin = tablePoseStamped.pose.position;

                // get the point of co
                geometry_msgs::PoseStamped coPoseStamped;
                if (!extractPoseStampedFromCollisionObject(co, coPoseStamped))
                {
                	ROS_ERROR("StateCreatorFromPlanningScene::%s: object:%s does not have a pose!", __func__, co.id.c_str());
                	return;
                }
                const geometry_msgs::Point& coPoint = coPoseStamped.pose.position;

                // co is beneath table
                if (origin.z > coPoint.z)
                    continue;
                // simplified: find table with smallest distance to object
                double distance = hypot(coPoint.x - origin.x, coPoint.y - origin.y);
                if (distance < closest_distance)
                {
                    closest_distance = distance;
                    closest_table = table.id;
                }
            }
        }
        if (closest_table != "table") // found a matching table
        {
            ROS_DEBUG_STREAM("putting " << co.id << " on " << closest_table);
            Predicate pOn;
            pOn.name = "object-on";
            pOn.parameters.push_back(co.id);
            pOn.parameters.push_back(closest_table);
            currentState.setBooleanPredicate(pOn.name, pOn.parameters, true);
        }
        else
        	ROS_WARN("StateCreatorFromPlanningScene::%s: NO matching Table found for object: %s",
        			__func__, co.id.c_str());
    }

//    std::pair<double, double> StateCreatorFromPlanningScene::distanceBetweenTwoPoses(const geometry_msgs::PoseStamped & posePS,
//            const geometry_msgs::PoseStamped & poseState)
//    {
//        // OR poses might be in a sensor frame -> transform to PS frame first
//        geometry_msgs::PoseStamped poseOR_transformed;
//        try {
//            tf_.waitForTransform(posePS.header.frame_id, poseState.header.frame_id, poseState.header.stamp,
//                    ros::Duration(0.5));
//            tf_.transformPose(posePS.header.frame_id, poseState, poseOR_transformed);
//        } catch (tf::TransformException &ex) {
//            ROS_ERROR("%s", ex.what());
//        }
//
//		ROS_DEBUG_STREAM("StateCreatorFromPlanningScene::" << __func__ << ": frame ObjPlanningScene: "
//				<< posePS.header.frame_id << ": frame ObjSymbolicState: "
//				<< poseState.header.frame_id);
//        tf::Pose tfPS;
//        tf::Pose tfState;
//        tf::poseMsgToTF(posePS.pose, tfPS);
//        tf::poseMsgToTF(poseState.pose, tfState);
//        tf::Pose delta = tfPS.inverseTimes(tfState);
//        return std::make_pair(hypot(delta.getOrigin().x(), delta.getOrigin().y()),
//                fabs(delta.getOrigin().z()));   // usually we're interested in the 2d distance independently
//    }

};

<commit_msg>removing all movable object from sym state<commit_after>#include "tidyup_state_creators/stateCreatorFromPlanningScene.h"
#include "tidyup_utils/stringutil.h"
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <moveit/move_group/capability_names.h>
#include <moveit_msgs/GetPlanningScene.h>
#include <moveit_msgs/PlanningScene.h>

PLUGINLIB_EXPORT_CLASS(tidyup_state_creators::StateCreatorFromPlanningScene, continual_planning_executive::StateCreator)

namespace tidyup_state_creators
{
    StateCreatorFromPlanningScene::StateCreatorFromPlanningScene()
    {
    	ros::NodeHandle nh;
    	srvPlanningScene_ = nh.serviceClient<moveit_msgs::GetPlanningScene>(move_group::GET_PLANNING_SCENE_SERVICE_NAME);
    }

    StateCreatorFromPlanningScene::~StateCreatorFromPlanningScene()
    {
    }

    void StateCreatorFromPlanningScene::initialize(const std::deque<std::string>& arguments)
    {
    }

    bool StateCreatorFromPlanningScene::fillState(SymbolicState& state)
    {
    	initializePlanningScene();
    	// Tables have been added to symbolic state in goalCreatorLoadTablesIntoPlanningScene
    	initializeTables(state);

    	// delete all movable objects from symbolic state, and re-add the important objects later (from PS)
		const multimap<string, string> objects = state.getTypedObjects();
		std::pair<multimap<string, string>::const_iterator, multimap<string, string>::const_iterator> iterators =
		        objects.equal_range("movable_object");
		for (multimap<string, string>::const_iterator it = iterators.first; it != iterators.second; it++)
		{
			// also predicates with the objects are removed
			state.removeObject(it->second);
		}

    	ROS_DEBUG_STREAM("StateCreatorFromPlanningScene::" << __func__ << ": number of collision objects in planning scene: "
    			<< planningScene_.world.collision_objects.size());

    	forEach(const moveit_msgs::CollisionObject& object, planningScene_.world.collision_objects)
		{
    		ROS_DEBUG_STREAM("StateCreatorFromPlanningScene::" << __func__ << ": processing object: " << object.id);

    		if (StringUtil::startsWith(object.id, "table"))
    		{
    			// tables are already in symbolic state - load in goalCreatorLoadTablesIntoPlanningScene
    			continue;
    		}
            if (StringUtil::startsWith(object.id, "door"))
            {
                continue;
            }
            if (StringUtil::startsWith(object.id, "sponge"))
            {
                continue;
            }
    		addObjectToSymbolicState(state, object, "movable_object");
    		// add object-on predicate to object
    		findMatchingTable(state, planningScene_.world.collision_objects, object);
		}
    	// attached objects
    	forEach(const moveit_msgs::AttachedCollisionObject& attachedObject, planningScene_.robot_state.attached_collision_objects)
    	{
    		const moveit_msgs::CollisionObject& object = attachedObject.object;
    		addObjectToSymbolicState(state, object, "movable_object");

            // grasped predicate
            vector<string> params;
            params.push_back(object.id);
            params.push_back("arm_name");
            if (StringUtil::startsWith(attachedObject.link_name, "l_"))
            {
                ROS_DEBUG_STREAM("processing attached object " << object.id << " on left_arm.");
                params[1] = "left_arm";
                state.setBooleanPredicate("object-grasped", params, true);
            }
            else if (StringUtil::startsWith(attachedObject.link_name, "r_"))
            {
                ROS_DEBUG_STREAM("processing attached object " << object.id << " on right_arm.");
                params[1] = "right_arm";
                state.setBooleanPredicate("object-grasped", params, true);
            }
            else
            {
                ROS_ERROR_STREAM("processing attached object " << object.id << " on unknown link.");
            }
    	}

    	return true;
    }

    void StateCreatorFromPlanningScene::initializePlanningScene()
    {
        ROS_INFO("StateCreatorFromPlanningScene::%s: Waiting for %s service.",
        		__func__, move_group::GET_PLANNING_SCENE_SERVICE_NAME.c_str());
        srvPlanningScene_.waitForExistence();

        moveit_msgs::GetPlanningScene::Request request;
        moveit_msgs::GetPlanningScene::Response response;
        //request.components.WORLD_OBJECT_NAMES; IMPORTANT: This declaration does not work!
        request.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY |
            moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS;

        if (!srvPlanningScene_.call(request, response))
        {
            ROS_ERROR("StateCreatorFromPlanningScene::%s: Failed to get initial planning scene.", __func__);
        }

        ROS_DEBUG("StateCreatorFromPlanningScene::%s: Number of collision objects: %lu",
        		__func__, response.scene.world.collision_objects.size());

        setPlanningScene(response.scene);
    }

    void StateCreatorFromPlanningScene::setPlanningScene(const moveit_msgs::PlanningScene& scene)
    {
    	planningScene_ = scene;
    }

    void StateCreatorFromPlanningScene::initializeTables(const SymbolicState& currentState)
    {
        ROS_DEBUG_STREAM("processing tables");
        pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> tablesRange =
                currentState.getTypedObjects().equal_range("table");
        for (SymbolicState::TypedObjectConstIterator tablesIterator = tablesRange.first;
                tablesIterator != tablesRange.second; tablesIterator++)
        {
            ROS_DEBUG_STREAM("processing "<<tablesIterator->second);
            tables_.insert(tablesIterator->second);
//            pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> locationsRange =
//                    currentState.getTypedObjects().equal_range("manipulation_location");
//            for (SymbolicState::TypedObjectConstIterator locationsIterator = locationsRange.first;
//                    locationsIterator != locationsRange.second; locationsIterator++)
//            {
//                // (location-near-table ?l - manipulation-location ?t - table)
//                Predicate pAt;
//                pAt.name = "location-near-table";
//                pAt.parameters.push_back(locationsIterator->second);
//                pAt.parameters.push_back(tablesIterator->second);
//                bool value = false;
//                currentState.hasBooleanPredicate(pAt, &value);
//                if (value)
//                {
//                    ROS_DEBUG_STREAM("adding location "<<locationsIterator->second<<" to "<<tablesIterator->second);
//                    tableLocations_.insert(make_pair(tablesIterator->second, locationsIterator->second));
//                }
//            }
        }
    }

    bool StateCreatorFromPlanningScene::extractPoseStampedFromCollisionObject(const moveit_msgs::CollisionObject& co,
    		geometry_msgs::PoseStamped& pose) const
    {
    	if (co.mesh_poses.empty() && co.primitive_poses.empty())
    	{
    		ROS_WARN("stateCreatorFromPlanningScene::%s: CollisionObject %s had no mesh_poses nor primitive_poses", __func__, co.id.c_str());
    		return false;
    	}

        std::vector<geometry_msgs::Pose> poses;
        if(!co.mesh_poses.empty() && !co.primitive_poses.empty()) {
            ROS_WARN("%s: CollisionObject %s had mesh_poses and primitive_poses -> using primitive_poses",
                    __func__, co.id.c_str());
            poses = co.primitive_poses;
        } else if(!co.primitive_poses.empty()) {
            poses = co.primitive_poses;
        } else {
            ROS_ASSERT(!co.mesh_poses.empty());
            poses = co.mesh_poses;
        }

        if(poses.size() > 1) {
            ROS_WARN("%s: CollisionObject %s had %zu poses -> using first.", __func__, co.id.c_str(), poses.size());
        }
        pose.pose = poses.front();
        pose.header = co.header;
    	return true;
    }

    void StateCreatorFromPlanningScene::addObjectToSymbolicState(SymbolicState& state, const moveit_msgs::CollisionObject& co,
    		const std::string& objectType)
    {
    	// Verify that objectType is spelled correctly
    	if (!doesObjectTypeExist(objectType))
    		return;
    	state.addObject(co.id, objectType);
        state.setNumericalFluent("timestamp", co.id, co.header.stamp.toSec());
        state.addObject(co.header.frame_id, "frameid");
        ROS_DEBUG_STREAM("StateCreatorFromPlanningScene::" << __func__ << " object: " << co.id
        		<< " has frame: " << co.header.frame_id);
        state.setObjectFluent("frame-id", co.id, co.header.frame_id);

        geometry_msgs::PoseStamped poseStamped;
        if (!extractPoseStampedFromCollisionObject(co, poseStamped))
        {
        	ROS_ERROR("StateCreatorFromPlanningScene::%s: object:%s does not have a pose!", __func__, co.id.c_str());
        	return;
        }
        geometry_msgs::Pose pose = poseStamped.pose;
        state.setNumericalFluent("x", co.id, pose.position.x);
        state.setNumericalFluent("y", co.id, pose.position.y);
        state.setNumericalFluent("z", co.id, pose.position.z);
        state.setNumericalFluent("qx", co.id, pose.orientation.x);
        state.setNumericalFluent("qy", co.id, pose.orientation.y);
        state.setNumericalFluent("qz", co.id, pose.orientation.z);
        state.setNumericalFluent("qw", co.id, pose.orientation.w);
    }

    bool StateCreatorFromPlanningScene::doesObjectTypeExist(const string& objectType)
    {
    	std::string types[] = { "pose", "frameid", "location", "manipulation_location",
    							"table", "movable_object", "arm", "arm_state" };
    	std::set<std::string> objectTypes(types, types + sizeof(types) / sizeof(types[0]));
    	ROS_ASSERT(objectTypes.size() == 8);

    	if (objectTypes.find(objectType) != objectTypes.end())
    	{
    		return true;
    	}
    	else
    	{
    		ROS_ERROR("StateCreatorFromPlanningScene::%s: Object Type %s does not exist "
    				"- maybe typo", __func__, objectType.c_str());
    		return false;
    	}
    }

    void StateCreatorFromPlanningScene::findMatchingTable(SymbolicState& currentState,
    		const std::vector<moveit_msgs::CollisionObject>& allCos,
    		const moveit_msgs::CollisionObject& co)
    {
        string closest_table = "table";
        double closest_distance = 2.0;
        forEach(const moveit_msgs::CollisionObject& table, allCos)
        {
        	// if collisionObject table is really a table (was added in initializedTables())
            if (tables_.find(table.id) != tables_.end())
            {
                geometry_msgs::PoseStamped tablePoseStamped;
                if (!extractPoseStampedFromCollisionObject(table, tablePoseStamped))
                {
                	ROS_ERROR("StateCreatorFromPlanningScene::%s: table:%s does not have a pose!", __func__, table.id.c_str());
                	return;
                }
                const geometry_msgs::Point& origin = tablePoseStamped.pose.position;

                // get the point of co
                geometry_msgs::PoseStamped coPoseStamped;
                if (!extractPoseStampedFromCollisionObject(co, coPoseStamped))
                {
                	ROS_ERROR("StateCreatorFromPlanningScene::%s: object:%s does not have a pose!", __func__, co.id.c_str());
                	return;
                }
                const geometry_msgs::Point& coPoint = coPoseStamped.pose.position;

                // co is beneath table
                if (origin.z > coPoint.z)
                    continue;
                // simplified: find table with smallest distance to object
                double distance = hypot(coPoint.x - origin.x, coPoint.y - origin.y);
                if (distance < closest_distance)
                {
                    closest_distance = distance;
                    closest_table = table.id;
                }
            }
        }
        if (closest_table != "table") // found a matching table
        {
            ROS_DEBUG_STREAM("putting " << co.id << " on " << closest_table);
            Predicate pOn;
            pOn.name = "object-on";
            pOn.parameters.push_back(co.id);
            pOn.parameters.push_back(closest_table);
            currentState.setBooleanPredicate(pOn.name, pOn.parameters, true);
        }
        else
        	ROS_WARN("StateCreatorFromPlanningScene::%s: NO matching Table found for object: %s",
        			__func__, co.id.c_str());
    }

//    std::pair<double, double> StateCreatorFromPlanningScene::distanceBetweenTwoPoses(const geometry_msgs::PoseStamped & posePS,
//            const geometry_msgs::PoseStamped & poseState)
//    {
//        // OR poses might be in a sensor frame -> transform to PS frame first
//        geometry_msgs::PoseStamped poseOR_transformed;
//        try {
//            tf_.waitForTransform(posePS.header.frame_id, poseState.header.frame_id, poseState.header.stamp,
//                    ros::Duration(0.5));
//            tf_.transformPose(posePS.header.frame_id, poseState, poseOR_transformed);
//        } catch (tf::TransformException &ex) {
//            ROS_ERROR("%s", ex.what());
//        }
//
//		ROS_DEBUG_STREAM("StateCreatorFromPlanningScene::" << __func__ << ": frame ObjPlanningScene: "
//				<< posePS.header.frame_id << ": frame ObjSymbolicState: "
//				<< poseState.header.frame_id);
//        tf::Pose tfPS;
//        tf::Pose tfState;
//        tf::poseMsgToTF(posePS.pose, tfPS);
//        tf::poseMsgToTF(poseState.pose, tfState);
//        tf::Pose delta = tfPS.inverseTimes(tfState);
//        return std::make_pair(hypot(delta.getOrigin().x(), delta.getOrigin().y()),
//                fabs(delta.getOrigin().z()));   // usually we're interested in the 2d distance independently
//    }

};

<|endoftext|>


<commit_before>#include "mlp_cv.hpp"

/// PROJECT
#include <csapex/msg/io.h>
#include <csapex/utility/register_apex_plugin.h>
#include <csapex/param/parameter_factory.h>
#include <csapex/model/node_modifier.h>
#include <csapex/msg/generic_vector_message.hpp>
#include <csapex/signal/slot.h>
#include <QFile>
CSAPEX_REGISTER_CLASS(csapex::MLPCv, csapex::Node)

using namespace csapex;
using namespace csapex::connection_types;

MLPCv::MLPCv()
    : loaded_(false)
{
}

void MLPCv::setup(NodeModifier &node_modifier)
{
    input_ = node_modifier.addInput<GenericVectorMessage, FeaturesMessage>("Unclassified feature");
    output_ = node_modifier.addOutput<GenericVectorMessage, FeaturesMessage>("Classified features");

    reload_ = node_modifier.addSlot("Reload", std::bind(&MLPCv::reloadMLP, this));
}

void MLPCv::setupParameters(Parameterizable &parameters)
{
    parameters.addParameter(csapex::param::ParameterFactory::declareFileInputPath("file", "mlp.yaml"),
                            [this](param::Parameter* p) {
        auto path = p->as<std::string>();
        if(path != path_){
            path_ = path;
            reloadMLP();
        }
    });
}

void MLPCv::loadMLP()
{
#if CV_MAJOR_VERSION == 2
    mlp_.load(path_.c_str());
    loaded_ = mlp_.get_layer_count() > 0; // oddly opencv does not check if file is valid
#elif CV_MAJOR_VERSION == 3
    mlp_ = cv::ml::ANN_MLP::load(path_);
    cv::Mat sizes = mlp_->getLayerSizes();
    loaded_ = sizes.rows > 0 || sizes.cols > 0;

#endif

}

void MLPCv::process()
{
    std::shared_ptr<std::vector<FeaturesMessage> const> input =
            msg::getMessage<GenericVectorMessage, FeaturesMessage>(input_);

    std::shared_ptr<std::vector<FeaturesMessage>> output(new std::vector<FeaturesMessage>);
    if(!loaded_) {
        if(QFile(QString::fromStdString(path_)).exists()) {
            loadMLP();
        }
    }

    if(loaded_) {

        std::size_t n = input->size();
        output->resize(n);
        for(std::size_t i = 0; i < n; ++i) {
            classify(input->at(i),output->at(i));
        }

    } else {
        *output = *input;
        node_modifier_->setWarning("cannot classfiy, no MLP loaded");
    }

    msg::publish<GenericVectorMessage, FeaturesMessage>(output_, output);
}

void MLPCv::classify(const FeaturesMessage &input,
                     FeaturesMessage &output)
{
    output = input;

    cv::Mat feature(1, input.value.size(),cv::DataType<float>::type);
    for(std::size_t i = 0; i < input.value.size(); ++i){
        feature.at<float>(0,i) = input.value[i];
    }
    cv::Mat response;

#if CV_MAJOR_VERSION == 2
    mlp_.predict(feature, response);
#elif CV_MAJOR_VERSION == 3
    mlp_->predict(feature, response);
#endif

    cv::Point max;
    cv::minMaxLoc(response, nullptr, nullptr, nullptr, &max);

    output.classification = max.x;
}

void MLPCv::reloadMLP()
{
    loaded_ = false;
}
<commit_msg>mlp do not use qfile<commit_after>#include "mlp_cv.hpp"

/// PROJECT
#include <csapex/msg/io.h>
#include <csapex/utility/register_apex_plugin.h>
#include <csapex/param/parameter_factory.h>
#include <csapex/model/node_modifier.h>
#include <csapex/msg/generic_vector_message.hpp>
#include <csapex/signal/slot.h>

CSAPEX_REGISTER_CLASS(csapex::MLPCv, csapex::Node)

using namespace csapex;
using namespace csapex::connection_types;

MLPCv::MLPCv()
    : loaded_(false)
{
}

void MLPCv::setup(NodeModifier &node_modifier)
{
    input_ = node_modifier.addInput<GenericVectorMessage, FeaturesMessage>("Unclassified feature");
    output_ = node_modifier.addOutput<GenericVectorMessage, FeaturesMessage>("Classified features");

    reload_ = node_modifier.addSlot("Reload", std::bind(&MLPCv::reloadMLP, this));
}

void MLPCv::setupParameters(Parameterizable &parameters)
{
    parameters.addParameter(csapex::param::ParameterFactory::declareFileInputPath("file", "mlp.yaml"),
                            [this](param::Parameter* p) {
        auto path = p->as<std::string>();
        if(path != path_){
            path_ = path;
            reloadMLP();
        }
    });
}

void MLPCv::loadMLP()
{
#if CV_MAJOR_VERSION == 2
    mlp_.load(path_.c_str());
    loaded_ = mlp_.get_layer_count() > 0; // oddly opencv does not check if file is valid
#elif CV_MAJOR_VERSION == 3
    mlp_ = cv::ml::ANN_MLP::load(path_);
    cv::Mat sizes = mlp_->getLayerSizes();
    loaded_ = sizes.rows > 0 || sizes.cols > 0;

#endif

}

void MLPCv::process()
{
    std::shared_ptr<std::vector<FeaturesMessage> const> input =
            msg::getMessage<GenericVectorMessage, FeaturesMessage>(input_);

    std::shared_ptr<std::vector<FeaturesMessage>> output(new std::vector<FeaturesMessage>);
    if(!loaded_) {
        if(path_ != "") {
            loadMLP();
        }
    }

    if(loaded_) {

        std::size_t n = input->size();
        output->resize(n);
        for(std::size_t i = 0; i < n; ++i) {
            classify(input->at(i),output->at(i));
        }

    } else {
        *output = *input;
        node_modifier_->setWarning("cannot classfiy, no MLP loaded");
    }

    msg::publish<GenericVectorMessage, FeaturesMessage>(output_, output);
}

void MLPCv::classify(const FeaturesMessage &input,
                     FeaturesMessage &output)
{
    output = input;

    cv::Mat feature(1, input.value.size(),cv::DataType<float>::type);
    for(std::size_t i = 0; i < input.value.size(); ++i){
        feature.at<float>(0,i) = input.value[i];
    }
    cv::Mat response;

#if CV_MAJOR_VERSION == 2
    mlp_.predict(feature, response);
#elif CV_MAJOR_VERSION == 3
    mlp_->predict(feature, response);
#endif

    cv::Point max;
    cv::minMaxLoc(response, nullptr, nullptr, nullptr, &max);

    output.classification = max.x;
}

void MLPCv::reloadMLP()
{
    loaded_ = false;
}
<|endoftext|>