<commit_before>#ifndef DUNE_MULTISCALE_SRC_COMMON_HH
#define DUNE_MULTISCALE_SRC_COMMON_HH

#ifdef HAVE_CMAKE_CONFIG
 #include "cmake_config.h"
#elif defined (HAVE_CONFIG_H)
 #include <config.h>
#endif // ifdef HAVE_CMAKE_CONFIG

#undef ENABLE_MPI

// polynomial order of discrete space
#ifndef POLORDER
  #define POLORDER 1
#endif

#ifndef USE_GRAPE
 #define USE_GRAPE HAVE_GRAPE
#endif

#define USE_TWISTFREE_MAPPER
#define VERBOSE false

#include <iostream>
#include <sstream>

#include <stdio.h>
#include <stdlib.h>
// -----------------------------

#include <dune/common/mpihelper.hh> // An initializer of MPI
#include <dune/common/exceptions.hh> // We use exceptions
#include <dune/common/unused.hh>

#if HAVE_GRAPE
 #include <dune/grid/io/visual/grapedatadisplay.hh>
#endif

// to display data with ParaView:
#include <dune/grid/io/file/vtk/vtkwriter.hh>

#include <dune/fem/io/file/dataoutput.hh>
#include <dune/fem/io/parameter.hh>
#include <dune/fem/io/file/datawriter.hh>
#include <dune/fem/gridpart/common/gridpart.hh>
#include <dune/multiscale/grids/periodicgridpart/periodicgridpart.hh>
#include <dune/fem/gridpart/adaptiveleafgridpart.hh>
#include <dune/fem/space/lagrangespace.hh>
#include <dune/fem/function/adaptivefunction.hh>
#include <dune/fem/operator/matrix/spmatrix.hh>
#include <dune/fem/space/common/adaptmanager.hh>
#include <dune/fem/solver/inverseoperators.hh>
#include <dune/fem/misc/mpimanager.hh>

#include <dune/stuff/common/parameter/configcontainer.hh>
#include <dune/stuff/common/debug.hh>
#include <dune/stuff/common/misc.hh>
#include <dune/stuff/common/logging.hh>
#include <dune/stuff/common/profiler.hh>

void init(int argc, char** argv) {
  namespace DSC = Dune::Stuff::Common;
  Dune::MPIManager::initialize(argc, argv);
  DSC::Config().readCommandLine(argc, argv);

  // LOG_NONE = 1, LOG_ERROR = 2, LOG_INFO = 4,LOG_DEBUG = 8,LOG_CONSOLE = 16,LOG_FILE = 32
  // --> LOG_ERROR | LOG_INFO | LOG_DEBUG | LOG_CONSOLE | LOG_FILE = 62
  const bool useLogger = false;
  DSC::Logger().create(DSC_CONFIG_GETB("logging.level", 62, useLogger),
                  DSC_CONFIG_GETB("logging.file", std::string(argv[0]) + ".log", useLogger),
                  DSC_CONFIG_GETB("global.datadir", "data", useLogger),
                  DSC_CONFIG_GETB("logging.dir", "log" /*path below datadir*/, useLogger)
                  );
  DSC_CONFIG.setRecordDefaults(true);
  DSC_PROFILER.setOutputdir(DSC_CONFIG_GET("global.datadir", "data"));
} // init

#endif // DUNE_MULTISCALE_SRC_COMMON_HH
<commit_msg>Code runs on Mac again.<commit_after>#ifndef DUNE_MULTISCALE_SRC_COMMON_HH
#define DUNE_MULTISCALE_SRC_COMMON_HH

#ifdef HAVE_CMAKE_CONFIG
 #include "cmake_config.h"
#elif defined (HAVE_CONFIG_H)
 #include <config.h>
#endif // ifdef HAVE_CMAKE_CONFIG

// polynomial order of discrete space
#ifndef POLORDER
  #define POLORDER 1
#endif

#ifndef USE_GRAPE
 #define USE_GRAPE HAVE_GRAPE
#endif

#define USE_TWISTFREE_MAPPER
#define VERBOSE false

#include <iostream>
#include <sstream>

#include <stdio.h>
#include <stdlib.h>
// -----------------------------

#include <dune/common/mpihelper.hh> // An initializer of MPI
#include <dune/common/exceptions.hh> // We use exceptions
#include <dune/common/unused.hh>

#if HAVE_GRAPE
 #include <dune/grid/io/visual/grapedatadisplay.hh>
#endif

// to display data with ParaView:
#include <dune/grid/io/file/vtk/vtkwriter.hh>

#include <dune/fem/io/file/dataoutput.hh>
#include <dune/fem/io/parameter.hh>
#include <dune/fem/io/file/datawriter.hh>
#include <dune/fem/gridpart/common/gridpart.hh>
#include <dune/multiscale/grids/periodicgridpart/periodicgridpart.hh>
#include <dune/fem/gridpart/adaptiveleafgridpart.hh>
#include <dune/fem/space/lagrangespace.hh>
#include <dune/fem/function/adaptivefunction.hh>
#include <dune/fem/operator/matrix/spmatrix.hh>
#include <dune/fem/space/common/adaptmanager.hh>
#include <dune/fem/solver/inverseoperators.hh>
#include <dune/fem/misc/mpimanager.hh>

#include <dune/stuff/common/parameter/configcontainer.hh>
#include <dune/stuff/common/debug.hh>
#include <dune/stuff/common/misc.hh>
#include <dune/stuff/common/logging.hh>
#include <dune/stuff/common/profiler.hh>

void init(int argc, char** argv) {
  namespace DSC = Dune::Stuff::Common;
  Dune::MPIManager::initialize(argc, argv);
  DSC::Config().readCommandLine(argc, argv);

  // LOG_NONE = 1, LOG_ERROR = 2, LOG_INFO = 4,LOG_DEBUG = 8,LOG_CONSOLE = 16,LOG_FILE = 32
  // --> LOG_ERROR | LOG_INFO | LOG_DEBUG | LOG_CONSOLE | LOG_FILE = 62
  const bool useLogger = false;
  DSC::Logger().create(DSC_CONFIG_GETB("logging.level", 62, useLogger),
                  DSC_CONFIG_GETB("logging.file", std::string(argv[0]) + ".log", useLogger),
                  DSC_CONFIG_GETB("global.datadir", "data", useLogger),
                  DSC_CONFIG_GETB("logging.dir", "log" /*path below datadir*/, useLogger)
                  );
  DSC_CONFIG.setRecordDefaults(true);
  DSC_PROFILER.setOutputdir(DSC_CONFIG_GET("global.datadir", "data"));
} // init

#endif // DUNE_MULTISCALE_SRC_COMMON_HH
<|endoftext|>


<commit_before>// Time:  O(n)
// Space: O(h)

/**
 * Definition of TreeNode:
 * class TreeNode {
 * public:
 *     int val;
 *     TreeNode *left, *right;
 *     TreeNode(int val) {
 *         this->val = val;
 *         this->left = this->right = NULL;
 *     }
 * }
 */
class Solution {
public:
    /**
     * This method will be invoked first, you should design your own algorithm
     * to serialize a binary tree which denote by a root node to a string which
     * can be easily deserialized by your own "deserialize" method later.
     */
    string serialize(TreeNode *root) {
        ostringstream out;
        serializeHelper(root, out);
        return out.str();
    }

    /**
     * This method will be invoked second, the argument data is what exactly
     * you serialized at method "serialize", that means the data is not given by
     * system, it's given by your own serialize method. So the format of data is
     * designed by yourself, and deserialize it here as you serialize it in
     * "serialize" method.
     */
    TreeNode *deserialize(string data) {
        istringstream in(data);
        return deserializeHelper(in);
    }

private:
    void serializeHelper(const TreeNode *root, ostringstream& out) {
        if (!root)  {
            out << "# ";
        } else {
            out << root->val << " ";
            serializeHelper(root->left, out);
            serializeHelper(root->right, out);
        }
    }

    TreeNode *deserializeHelper(istringstream& in) {
        string val;
        in >> val;
        if (val == "#") {
            return nullptr;
        } else {
            TreeNode* root = new TreeNode(stoi(val));
            root->left = deserializeHelper(in);
            root->right = deserializeHelper(in);
            return root;
        }
    }
};
<commit_msg>Update binary-tree-serialization.cpp<commit_after>// Time:  O(n)
// Space: O(h)

/**
 * Definition of TreeNode:
 * class TreeNode {
 * public:
 *     int val;
 *     TreeNode *left, *right;
 *     TreeNode(int val) {
 *         this->val = val;
 *         this->left = this->right = NULL;
 *     }
 * }
 */
class Solution {
public:
    /**
     * This method will be invoked first, you should design your own algorithm
     * to serialize a binary tree which denote by a root node to a string which
     * can be easily deserialized by your own "deserialize" method later.
     */
    string serialize(TreeNode *root) {
        string output;
        serializeHelper(root, &output);
        return output;
    }

    /**
     * This method will be invoked second, the argument data is what exactly
     * you serialized at method "serialize", that means the data is not given by
     * system, it's given by your own serialize method. So the format of data is
     * designed by yourself, and deserialize it here as you serialize it in
     * "serialize" method.
     */
    TreeNode *deserialize(string data) {
        TreeNode *root = nullptr;
        int start = 0;
        deserializeHelper(data, &start, &root);
        return root;
    }

private:
    bool getNumber(const string &data, int *start, int *num) {
        int sign = 1;
        if (data[*start] == '#') {
            *start += 2;  // Skip "# ".
            return false;
        } else if (data[*start] == '-') {
            sign = -1;
            ++(*start);
        }

        for (*num = 0; isdigit(data[*start]); ++(*start)) {
            *num = *num * 10 + data[*start] - '0';
        }
        *num *= sign;
        ++(*start);  // Skip " ".

        return true;
    }

    void serializeHelper(const TreeNode *root, string *prev) {
        if (!root)  {
            prev->append("# ");
        } else {
            prev->append(to_string(root->val).append(" "));
            serializeHelper(root->left, prev);
            serializeHelper(root->right, prev);
        }
    }

    void deserializeHelper(const string& data,
                           int *start,  TreeNode **root) {
        int num;
        if (!getNumber(data, start, &num)) {
            *root = nullptr;
        } else {
            *root = new TreeNode(num);
            deserializeHelper(data, start, &((*root)->left));
            deserializeHelper(data, start, &((*root)->right));
        }
    }
};

// Time:  O(n)
// Space: O(n)
class Solution2 {
public:
    /**
     * This method will be invoked first, you should design your own algorithm
     * to serialize a binary tree which denote by a root node to a string which
     * can be easily deserialized by your own "deserialize" method later.
     */
    string serialize(TreeNode *root) {
        ostringstream out;
        serializeHelper(root, out);
        return out.str();
    }

    /**
     * This method will be invoked second, the argument data is what exactly
     * you serialized at method "serialize", that means the data is not given by
     * system, it's given by your own serialize method. So the format of data is
     * designed by yourself, and deserialize it here as you serialize it in
     * "serialize" method.
     */
    TreeNode *deserialize(string data) {
        istringstream in(data);  // Space: O(n)
        return deserializeHelper(in);
    }

private:
    void serializeHelper(const TreeNode *root, ostringstream& out) {
        if (!root)  {
            out << "# ";
        } else {
            out << root->val << " ";
            serializeHelper(root->left, out);
            serializeHelper(root->right, out);
        }
    }

    TreeNode *deserializeHelper(istringstream& in) {
        string val;
        in >> val;
        if (val == "#") {
            return nullptr;
        } else {
            TreeNode* root = new TreeNode(stoi(val));
            root->left = deserializeHelper(in);
            root->right = deserializeHelper(in);
            return root;
        }
    }
};
<|endoftext|>


<commit_before>//FRAGMENT(includes)
#include <seqan/seeds.h>

using namespace seqan;

// FRAGMENT(writeSeed)
template<typename TSeed, typename TSeq>
void
writeSeed(TSeed & seed, TSeq const & seq0, TSeq const & seq1) {
    std::cout << "Seed from position " << leftPosition(seed, 0);
    std::cout << " to " << rightPosition(seed, 0) << ": ";
    std::cout << infix(seq0, leftPosition(seed, 0), rightPosition(seed, 0)+1) << std::endl;
    std::cout << "Seed from position " << leftPosition(seed, 1);
    std::cout << " to " << rightPosition(seed, 1) << ": ";
    std::cout << infix(seq1, leftPosition(seed, 1), rightPosition(seed, 1)+1) << std::endl;
}

//FRAGMENT(main)
int main() {
    typedef Seed<> TSeed;

    DnaString seq0 = "ATCATCAGTTATACTTTACCCAGGC";
    DnaString seq1 = "ATTCAGCATACTTTCCATGAAGC";

    TSeed seed(10, 7, 7);
    writeSeed(seed, seq0, seq1);

// FRAGMENT(extension)
    typedef int TScore;
    Score<TScore, Simple> scoreMatrix(1, -1, -1);
    TScore scoreDropOff = 1;
    extendSeed(seed, scoreDropOff, scoreMatrix, seq0, seq1, 2, GappedXDrop());

    std::cout << std::endl << "After extension:" << std::endl;
    writeSeed(seed, seq0, seq1);

// FRAGMENT(banded-alignment)
    Align<DnaString> align;
    resize(rows(alignment), 2);
    assignSource(row(align, 0), seq0);
    assignSource(row(align, 1), seq1);

    std::cout << std::endl << "Banded Alignment:" << std::endl;
    std::cout << "Score: " << bandedAlignment(align, seed, 2, scoreMatrix) << std::endl;
    std::cout << align;

// FRAGMENT(global-alignment)
    Align<DnaString> globalAlign;
    resize(rows(globalAlign), 2);
    assignSource(row(globalAlign, 0), seq0);
    assignSource(row(globalAlign, 1), seq1);

    std::cout << std::endl << "Global Alignment:" << std::endl;
    std::cout << "Score: " << globalAlignment(globalAlign, scoreMatrix) << std::endl;
    std::cout << globalAlign;

    return 0;
}
<commit_msg>small mistake in seed extension example<commit_after>//FRAGMENT(includes)
#include <seqan/seeds.h>

using namespace seqan;

// FRAGMENT(writeSeed)
template<typename TSeed, typename TSeq>
void
writeSeed(TSeed & seed, TSeq const & seq0, TSeq const & seq1) {
    std::cout << "Seed from position " << leftPosition(seed, 0);
    std::cout << " to " << rightPosition(seed, 0) << ": ";
    std::cout << infix(seq0, leftPosition(seed, 0), rightPosition(seed, 0)+1) << std::endl;
    std::cout << "Seed from position " << leftPosition(seed, 1);
    std::cout << " to " << rightPosition(seed, 1) << ": ";
    std::cout << infix(seq1, leftPosition(seed, 1), rightPosition(seed, 1)+1) << std::endl;
}

//FRAGMENT(main)
int main() {
    typedef Seed<> TSeed;

    DnaString seq0 = "ATCATCAGTTATACTTTACCCAGGC";
    DnaString seq1 = "ATTCAGCATACTTTCCATGAAGC";

    TSeed seed(10, 7, 7);
    writeSeed(seed, seq0, seq1);

// FRAGMENT(extension)
    typedef int TScore;
    Score<TScore, Simple> scoreMatrix(1, -1, -1);
    TScore scoreDropOff = 1;
    extendSeed(seed, scoreDropOff, scoreMatrix, seq0, seq1, 2, GappedXDrop());

    std::cout << std::endl << "After extension:" << std::endl;
    writeSeed(seed, seq0, seq1);

// FRAGMENT(banded-alignment)
    Align<DnaString> align;
    resize(rows(align), 2);
    assignSource(row(align, 0), seq0);
    assignSource(row(align, 1), seq1);

    std::cout << std::endl << "Banded Alignment:" << std::endl;
    std::cout << "Score: " << bandedAlignment(align, seed, 2, scoreMatrix) << std::endl;
    std::cout << align;

// FRAGMENT(global-alignment)
    Align<DnaString> globalAlign;
    resize(rows(globalAlign), 2);
    assignSource(row(globalAlign, 0), seq0);
    assignSource(row(globalAlign, 1), seq1);

    std::cout << std::endl << "Global Alignment:" << std::endl;
    std::cout << "Score: " << globalAlignment(globalAlign, scoreMatrix) << std::endl;
    std::cout << globalAlign;

    return 0;
}
<|endoftext|>


<commit_before>#ifndef STAN_MATH_OPENCL_KERNEL_GENERATOR_COLWISE_REDUCTION_HPP
#define STAN_MATH_OPENCL_KERNEL_GENERATOR_COLWISE_REDUCTION_HPP
#ifdef STAN_OPENCL

#include <stan/math/prim/meta.hpp>
#include <stan/math/opencl/opencl_context.hpp>
#include <stan/math/opencl/matrix_cl_view.hpp>
#include <stan/math/opencl/kernel_generator/type_str.hpp>
#include <stan/math/opencl/kernel_generator/name_generator.hpp>
#include <stan/math/opencl/kernel_generator/operation_cl.hpp>
#include <stan/math/opencl/kernel_generator/as_operation_cl.hpp>
#include <stan/math/opencl/kernel_generator/rowwise_reduction.hpp>
#include <stan/math/opencl/kernel_generator/calc_if.hpp>
#include <map>
#include <string>
#include <type_traits>
#include <utility>

namespace stan {
namespace math {
/** \addtogroup opencl_kernel_generator
 *  @{
 */

namespace internal {
class colwise_reduction_base {};

/**
 * Determine number of work groups in rows direction that will be run fro
 * colwise reduction of given size.
 * @param n_rows number of rows of expression to resuce
 * @param n_cols number of columns of expression to resuce
 * @return number of work groups in rows direction
 */
int colwise_reduction_wgs_rows(int n_rows, int n_cols) {
  int local = opencl_context.base_opts().at("LOCAL_SIZE_");
  int preferred_work_groups
      = opencl_context.device()[0].getInfo<CL_DEVICE_MAX_COMPUTE_UNITS>() * 16;
  // round up n_rows/local/n_cols
  return (std::min(preferred_work_groups, (n_rows + local - 1) / local) + n_cols
          - 1)
         / n_cols;
}
}  // namespace internal

/**
 * Represents a column wise reduction in kernel generator expressions. So as to
 * be efficient column wise reductions are only done partially. That means
 * instead of 1 row kernel output will have a few rows that need to be reduced
 * to obtain final result. This can be done in a separate kernel or after
 * copying to CPU. Also column wise reductions can not be used as arguments to
 * other operations - they can only be evaluated.
 * @tparam Derived derived type
 * @tparam T type of first argument
 * @tparam operation type with member function generate that accepts two
 * variable names and returns OpenCL source code for reduction operation_cl
 * @tparam PassZero whether \c operation passes trough zeros
 */
template <typename Derived, typename T, typename Operation>
class colwise_reduction
    : public internal::colwise_reduction_base,
      public operation_cl<Derived, typename std::remove_reference_t<T>::Scalar,
                          T> {
 public:
  using Scalar = typename std::remove_reference_t<T>::Scalar;
  using base = operation_cl<Derived, Scalar, T>;
  using base::var_name_;
  static const bool require_specific_local_size = true;

 protected:
  std::string init_;
  using base::derived;

 public:
  using base::cols;
  /**
   * Constructor
   * @param a the expression to reduce
   * @param init OpenCL source code of initialization value for reduction
   */
  explicit colwise_reduction(T&& a, const std::string& init)
      : base(std::forward<T>(a)), init_(init) {}

  /**
   * Generates kernel code for assigning this expression into result expression.
   * @param[in,out] generated map from (pointer to) already generated local
   * operations to variable names
   * @param[in,out] generated_all map from (pointer to) already generated all
   * operations to variable names
   * @param ng name generator for this kernel
   * @param row_index_name row index variable name
   * @param col_index_name column index variable name
   * @param result expression into which result is to be assigned
   * @return part of kernel with code for this and nested expressions
   */
  template <typename T_result>
  kernel_parts get_whole_kernel_parts(
      std::map<const void*, const char*>& generated,
      std::map<const void*, const char*>& generated_all, name_generator& ng,
      const std::string& row_index_name, const std::string& col_index_name,
      const T_result& result) const {
    kernel_parts parts = derived().get_kernel_parts(
        generated, generated_all, ng, row_index_name, col_index_name, false);
    kernel_parts out_parts = result.get_kernel_parts_lhs(
        generated, generated_all, ng, row_index_name, col_index_name);

    parts.args += out_parts.args;
    parts.reduction += "if (lid_i == 0) {\n"
                     + result.var_name_
                     + "_global[j * n_groups_i + wg_id_i] = "
                     + derived().var_name_ + "_local[0];\n"
                     "}\n";
    return parts;
  }

  /**
   * Generates kernel code for this and nested expressions.
   * @param row_index_name row index variable name
   * @param col_index_name column index variable name
   * @param view_handled whether whether caller already handled matrix view
   * @param var_name_arg name of the variable in kernel that holds argument to
   * this expression
   * @return part of kernel with code for this and nested expressions
   */
  inline kernel_parts generate(const std::string& row_index_name,
                               const std::string& col_index_name,
                               const bool view_handled,
                               const std::string& var_name_arg) const {
    kernel_parts res;
    res.declarations = "__local " + type_str<Scalar>() + " " + var_name_
                       + "_local[LOCAL_SIZE_];\n" + type_str<Scalar>() + " "
                       + var_name_ + ";\n";
    res.initialization = var_name_ + " = " + init_ + ";\n";
    res.body = var_name_ + " = " + Operation::generate(var_name_, var_name_arg)
               + ";\n";
    res.reduction =
          var_name_ + "_local[lid_i] = " + var_name_ + ";\n"
          "barrier(CLK_LOCAL_MEM_FENCE);\n"
          "for (int step = lsize_i / REDUCTION_STEP_SIZE; "
                "step > 0; step /= REDUCTION_STEP_SIZE) {\n"
          "  if (lid_i < step) {\n"
          "    for (int i = 1; i < REDUCTION_STEP_SIZE; i++) {\n"
          "      " + var_name_ + "_local[lid_i] = " +
        Operation::generate(var_name_ + "_local[lid_i]",
                            var_name_ + "_local[lid_i + step * i]") + ";\n"
          "    }\n"
          "  }\n"
          "  barrier(CLK_LOCAL_MEM_FENCE);\n"
          "}\n";
    return res;
  }

  /**
   * Number of rows of a matrix that would be the result of evaluating this
   * expression.
   * @return number of rows
   */
  inline int rows() const {
    int arg_rows = this->template get_arg<0>().rows();
    int arg_cols = this->template get_arg<0>().cols();
    if (arg_cols == 0) {
      return 1;
    }
    if (arg_cols == -1) {
      return -1;
    }
    return internal::colwise_reduction_wgs_rows(arg_rows, arg_cols);
  }

  /**
   * Number of rows threads need to be launched for.
   * @return number of rows
   */
  inline int thread_rows() const { return this->template get_arg<0>().rows(); }

  /**
   * Determine indices of extreme sub- and superdiagonals written.
   * @return pair of indices - bottom and top diagonal
   */
  inline std::pair<int, int> extreme_diagonals() const {
    return {-rows() + 1, cols() - 1};
  }
};  // namespace math

/**
 * Represents column wise sum - reduction in kernel generator expressions.
 * @tparam T type of expression
 */
template <typename T>
class colwise_sum_ : public colwise_reduction<colwise_sum_<T>, T, sum_op> {
  using base = colwise_reduction<colwise_sum_<T>, T, sum_op>;
  using base::arguments_;

 public:
  explicit colwise_sum_(T&& a)
      : colwise_reduction<colwise_sum_<T>, T, sum_op>(std::forward<T>(a), "0") {
  }
  /**
   * Creates a deep copy of this expression.
   * @return copy of \c *this
   */
  inline auto deep_copy() const {
    auto&& arg_copy = this->template get_arg<0>().deep_copy();
    return colwise_sum_<std::remove_reference_t<decltype(arg_copy)>>(
        std::move(arg_copy));
  }
};

/**
 * Column wise sum - reduction of a kernel generator expression. So as to
 * be efficient column wise reductions are only done partially. That means
 * instead of 1 row kernel output will have a few rows that need to be reduced
 * to obtain final result. This can be done in a separate kernel or after
 * copying to CPU. Also column wise reductions can not be used as arguments to
 * other operations - they can only be evaluated.
 * @tparam T type of input expression
 * @param a expression to reduce
 * @return sum
 */
template <typename T, require_all_kernel_expressions_t<T>* = nullptr>
inline auto colwise_sum(T&& a) {
  auto&& arg_copy = as_operation_cl(std::forward<T>(a)).deep_copy();
  return colwise_sum_<as_operation_cl_t<T>>(
      as_operation_cl(std::forward<T>(a)));
}

/**
 * Represents column wise max - reduction in kernel generator expressions.
 * @tparam T type of expression
 */
template <typename T>
class colwise_max_ : public colwise_reduction<
                         colwise_max_<T>, T,
                         max_op<typename std::remove_reference_t<T>::Scalar>> {
  using base
      = colwise_reduction<colwise_max_<T>, T,
                          max_op<typename std::remove_reference_t<T>::Scalar>>;
  using base::arguments_;

 public:
  using op = max_op<typename std::remove_reference_t<T>::Scalar>;
  explicit colwise_max_(T&& a)
      : colwise_reduction<colwise_max_<T>, T, op>(std::forward<T>(a),
                                                  op::init()) {}
  /**
   * Creates a deep copy of this expression.
   * @return copy of \c *this
   */
  inline auto deep_copy() const {
    auto&& arg_copy = this->template get_arg<0>().deep_copy();
    return colwise_max_<std::remove_reference_t<decltype(arg_copy)>>(
        std::move(arg_copy));
  }
};

/**
 * Column wise max - reduction of a kernel generator expression. So as to
 * be efficient column wise reductions are only done partially. That means
 * instead of 1 row kernel output will have a few rows that need to be reduced
 * to obtain final result. This can be done in a separate kernel or after
 * copying to CPU. Also column wise reductions can not be used as arguments to
 * other operations - they can only be evaluated.
 * @tparam T type of input expression
 * @param a expression to reduce
 * @return max
 */
template <typename T, require_all_kernel_expressions_t<T>* = nullptr>
inline auto colwise_max(T&& a) {
  auto&& arg_copy = as_operation_cl(std::forward<T>(a)).deep_copy();
  return colwise_max_<as_operation_cl_t<T>>(
      as_operation_cl(std::forward<T>(a)));
}

/**
 * Represents column wise min - reduction in kernel generator expressions.
 * @tparam T type of expression
 */
template <typename T>
class colwise_min_ : public colwise_reduction<
                         colwise_min_<T>, T,
                         min_op<typename std::remove_reference_t<T>::Scalar>> {
  using base
      = colwise_reduction<colwise_min_<T>, T,
                          min_op<typename std::remove_reference_t<T>::Scalar>>;
  using base::arguments_;

 public:
  using op = min_op<typename std::remove_reference_t<T>::Scalar>;
  explicit colwise_min_(T&& a)
      : colwise_reduction<colwise_min_<T>, T, op>(std::forward<T>(a),
                                                  op::init()) {}
  /**
   * Creates a deep copy of this expression.
   * @return copy of \c *this
   */
  inline auto deep_copy() const {
    auto&& arg_copy = this->template get_arg<0>().deep_copy();
    return colwise_min_<std::remove_reference_t<decltype(arg_copy)>>(
        std::move(arg_copy));
  }
};

/**
 * Column wise min - reduction of a kernel generator expression.  So as to
 * be efficient column wise reductions are only done partially. That means
 * instead of 1 row kernel output will have a few rows that need to be reduced
 * to obtain final result. This can be done in a separate kernel or after
 * copying to CPU. Also column wise reductions can not be used as arguments to
 * other operations - they can only be evaluated.
 * @tparam T type of input expression
 * @param a expression to reduce
 * @return min
 */
template <typename T, require_all_kernel_expressions_t<T>* = nullptr>
inline auto colwise_min(T&& a) {
  return colwise_min_<as_operation_cl_t<T>>(
      as_operation_cl(std::forward<T>(a)));
}

namespace internal {
template <typename T>
struct is_colwise_reduction_impl
    : public std::is_base_of<internal::colwise_reduction_base,
                             std::decay_t<T>> {};
template <typename T>
struct is_colwise_reduction_impl<calc_if_<true, T>>
    : public std::is_base_of<internal::colwise_reduction_base,
                             std::decay_t<T>> {};
}  // namespace internal

/**
 * Check whether a kernel generator expression is a colwise reduction.
 */
template <typename T>
using is_colwise_reduction
    = internal::is_colwise_reduction_impl<std::decay_t<T>>;

/** @}*/
}  // namespace math
}  // namespace stan
#endif
#endif
<commit_msg>fixed multiple translation units<commit_after>#ifndef STAN_MATH_OPENCL_KERNEL_GENERATOR_COLWISE_REDUCTION_HPP
#define STAN_MATH_OPENCL_KERNEL_GENERATOR_COLWISE_REDUCTION_HPP
#ifdef STAN_OPENCL

#include <stan/math/prim/meta.hpp>
#include <stan/math/opencl/opencl_context.hpp>
#include <stan/math/opencl/matrix_cl_view.hpp>
#include <stan/math/opencl/kernel_generator/type_str.hpp>
#include <stan/math/opencl/kernel_generator/name_generator.hpp>
#include <stan/math/opencl/kernel_generator/operation_cl.hpp>
#include <stan/math/opencl/kernel_generator/as_operation_cl.hpp>
#include <stan/math/opencl/kernel_generator/rowwise_reduction.hpp>
#include <stan/math/opencl/kernel_generator/calc_if.hpp>
#include <map>
#include <string>
#include <type_traits>
#include <utility>

namespace stan {
namespace math {
/** \addtogroup opencl_kernel_generator
 *  @{
 */

namespace internal {
class colwise_reduction_base {};

/**
 * Determine number of work groups in rows direction that will be run fro
 * colwise reduction of given size.
 * @param n_rows number of rows of expression to resuce
 * @param n_cols number of columns of expression to resuce
 * @return number of work groups in rows direction
 */
inline int colwise_reduction_wgs_rows(int n_rows, int n_cols) {
  int local = opencl_context.base_opts().at("LOCAL_SIZE_");
  int preferred_work_groups
      = opencl_context.device()[0].getInfo<CL_DEVICE_MAX_COMPUTE_UNITS>() * 16;
  // round up n_rows/local/n_cols
  return (std::min(preferred_work_groups, (n_rows + local - 1) / local) + n_cols
          - 1)
         / n_cols;
}
}  // namespace internal

/**
 * Represents a column wise reduction in kernel generator expressions. So as to
 * be efficient column wise reductions are only done partially. That means
 * instead of 1 row kernel output will have a few rows that need to be reduced
 * to obtain final result. This can be done in a separate kernel or after
 * copying to CPU. Also column wise reductions can not be used as arguments to
 * other operations - they can only be evaluated.
 * @tparam Derived derived type
 * @tparam T type of first argument
 * @tparam operation type with member function generate that accepts two
 * variable names and returns OpenCL source code for reduction operation_cl
 * @tparam PassZero whether \c operation passes trough zeros
 */
template <typename Derived, typename T, typename Operation>
class colwise_reduction
    : public internal::colwise_reduction_base,
      public operation_cl<Derived, typename std::remove_reference_t<T>::Scalar,
                          T> {
 public:
  using Scalar = typename std::remove_reference_t<T>::Scalar;
  using base = operation_cl<Derived, Scalar, T>;
  using base::var_name_;
  static const bool require_specific_local_size = true;

 protected:
  std::string init_;
  using base::derived;

 public:
  using base::cols;
  /**
   * Constructor
   * @param a the expression to reduce
   * @param init OpenCL source code of initialization value for reduction
   */
  explicit colwise_reduction(T&& a, const std::string& init)
      : base(std::forward<T>(a)), init_(init) {}

  /**
   * Generates kernel code for assigning this expression into result expression.
   * @param[in,out] generated map from (pointer to) already generated local
   * operations to variable names
   * @param[in,out] generated_all map from (pointer to) already generated all
   * operations to variable names
   * @param ng name generator for this kernel
   * @param row_index_name row index variable name
   * @param col_index_name column index variable name
   * @param result expression into which result is to be assigned
   * @return part of kernel with code for this and nested expressions
   */
  template <typename T_result>
  kernel_parts get_whole_kernel_parts(
      std::map<const void*, const char*>& generated,
      std::map<const void*, const char*>& generated_all, name_generator& ng,
      const std::string& row_index_name, const std::string& col_index_name,
      const T_result& result) const {
    kernel_parts parts = derived().get_kernel_parts(
        generated, generated_all, ng, row_index_name, col_index_name, false);
    kernel_parts out_parts = result.get_kernel_parts_lhs(
        generated, generated_all, ng, row_index_name, col_index_name);

    parts.args += out_parts.args;
    parts.reduction += "if (lid_i == 0) {\n"
                     + result.var_name_
                     + "_global[j * n_groups_i + wg_id_i] = "
                     + derived().var_name_ + "_local[0];\n"
                     "}\n";
    return parts;
  }

  /**
   * Generates kernel code for this and nested expressions.
   * @param row_index_name row index variable name
   * @param col_index_name column index variable name
   * @param view_handled whether whether caller already handled matrix view
   * @param var_name_arg name of the variable in kernel that holds argument to
   * this expression
   * @return part of kernel with code for this and nested expressions
   */
  inline kernel_parts generate(const std::string& row_index_name,
                               const std::string& col_index_name,
                               const bool view_handled,
                               const std::string& var_name_arg) const {
    kernel_parts res;
    res.declarations = "__local " + type_str<Scalar>() + " " + var_name_
                       + "_local[LOCAL_SIZE_];\n" + type_str<Scalar>() + " "
                       + var_name_ + ";\n";
    res.initialization = var_name_ + " = " + init_ + ";\n";
    res.body = var_name_ + " = " + Operation::generate(var_name_, var_name_arg)
               + ";\n";
    res.reduction =
          var_name_ + "_local[lid_i] = " + var_name_ + ";\n"
          "barrier(CLK_LOCAL_MEM_FENCE);\n"
          "for (int step = lsize_i / REDUCTION_STEP_SIZE; "
                "step > 0; step /= REDUCTION_STEP_SIZE) {\n"
          "  if (lid_i < step) {\n"
          "    for (int i = 1; i < REDUCTION_STEP_SIZE; i++) {\n"
          "      " + var_name_ + "_local[lid_i] = " +
        Operation::generate(var_name_ + "_local[lid_i]",
                            var_name_ + "_local[lid_i + step * i]") + ";\n"
          "    }\n"
          "  }\n"
          "  barrier(CLK_LOCAL_MEM_FENCE);\n"
          "}\n";
    return res;
  }

  /**
   * Number of rows of a matrix that would be the result of evaluating this
   * expression.
   * @return number of rows
   */
  inline int rows() const {
    int arg_rows = this->template get_arg<0>().rows();
    int arg_cols = this->template get_arg<0>().cols();
    if (arg_cols == 0) {
      return 1;
    }
    if (arg_cols == -1) {
      return -1;
    }
    return internal::colwise_reduction_wgs_rows(arg_rows, arg_cols);
  }

  /**
   * Number of rows threads need to be launched for.
   * @return number of rows
   */
  inline int thread_rows() const { return this->template get_arg<0>().rows(); }

  /**
   * Determine indices of extreme sub- and superdiagonals written.
   * @return pair of indices - bottom and top diagonal
   */
  inline std::pair<int, int> extreme_diagonals() const {
    return {-rows() + 1, cols() - 1};
  }
};  // namespace math

/**
 * Represents column wise sum - reduction in kernel generator expressions.
 * @tparam T type of expression
 */
template <typename T>
class colwise_sum_ : public colwise_reduction<colwise_sum_<T>, T, sum_op> {
  using base = colwise_reduction<colwise_sum_<T>, T, sum_op>;
  using base::arguments_;

 public:
  explicit colwise_sum_(T&& a)
      : colwise_reduction<colwise_sum_<T>, T, sum_op>(std::forward<T>(a), "0") {
  }
  /**
   * Creates a deep copy of this expression.
   * @return copy of \c *this
   */
  inline auto deep_copy() const {
    auto&& arg_copy = this->template get_arg<0>().deep_copy();
    return colwise_sum_<std::remove_reference_t<decltype(arg_copy)>>(
        std::move(arg_copy));
  }
};

/**
 * Column wise sum - reduction of a kernel generator expression. So as to
 * be efficient column wise reductions are only done partially. That means
 * instead of 1 row kernel output will have a few rows that need to be reduced
 * to obtain final result. This can be done in a separate kernel or after
 * copying to CPU. Also column wise reductions can not be used as arguments to
 * other operations - they can only be evaluated.
 * @tparam T type of input expression
 * @param a expression to reduce
 * @return sum
 */
template <typename T, require_all_kernel_expressions_t<T>* = nullptr>
inline auto colwise_sum(T&& a) {
  auto&& arg_copy = as_operation_cl(std::forward<T>(a)).deep_copy();
  return colwise_sum_<as_operation_cl_t<T>>(
      as_operation_cl(std::forward<T>(a)));
}

/**
 * Represents column wise max - reduction in kernel generator expressions.
 * @tparam T type of expression
 */
template <typename T>
class colwise_max_ : public colwise_reduction<
                         colwise_max_<T>, T,
                         max_op<typename std::remove_reference_t<T>::Scalar>> {
  using base
      = colwise_reduction<colwise_max_<T>, T,
                          max_op<typename std::remove_reference_t<T>::Scalar>>;
  using base::arguments_;

 public:
  using op = max_op<typename std::remove_reference_t<T>::Scalar>;
  explicit colwise_max_(T&& a)
      : colwise_reduction<colwise_max_<T>, T, op>(std::forward<T>(a),
                                                  op::init()) {}
  /**
   * Creates a deep copy of this expression.
   * @return copy of \c *this
   */
  inline auto deep_copy() const {
    auto&& arg_copy = this->template get_arg<0>().deep_copy();
    return colwise_max_<std::remove_reference_t<decltype(arg_copy)>>(
        std::move(arg_copy));
  }
};

/**
 * Column wise max - reduction of a kernel generator expression. So as to
 * be efficient column wise reductions are only done partially. That means
 * instead of 1 row kernel output will have a few rows that need to be reduced
 * to obtain final result. This can be done in a separate kernel or after
 * copying to CPU. Also column wise reductions can not be used as arguments to
 * other operations - they can only be evaluated.
 * @tparam T type of input expression
 * @param a expression to reduce
 * @return max
 */
template <typename T, require_all_kernel_expressions_t<T>* = nullptr>
inline auto colwise_max(T&& a) {
  auto&& arg_copy = as_operation_cl(std::forward<T>(a)).deep_copy();
  return colwise_max_<as_operation_cl_t<T>>(
      as_operation_cl(std::forward<T>(a)));
}

/**
 * Represents column wise min - reduction in kernel generator expressions.
 * @tparam T type of expression
 */
template <typename T>
class colwise_min_ : public colwise_reduction<
                         colwise_min_<T>, T,
                         min_op<typename std::remove_reference_t<T>::Scalar>> {
  using base
      = colwise_reduction<colwise_min_<T>, T,
                          min_op<typename std::remove_reference_t<T>::Scalar>>;
  using base::arguments_;

 public:
  using op = min_op<typename std::remove_reference_t<T>::Scalar>;
  explicit colwise_min_(T&& a)
      : colwise_reduction<colwise_min_<T>, T, op>(std::forward<T>(a),
                                                  op::init()) {}
  /**
   * Creates a deep copy of this expression.
   * @return copy of \c *this
   */
  inline auto deep_copy() const {
    auto&& arg_copy = this->template get_arg<0>().deep_copy();
    return colwise_min_<std::remove_reference_t<decltype(arg_copy)>>(
        std::move(arg_copy));
  }
};

/**
 * Column wise min - reduction of a kernel generator expression.  So as to
 * be efficient column wise reductions are only done partially. That means
 * instead of 1 row kernel output will have a few rows that need to be reduced
 * to obtain final result. This can be done in a separate kernel or after
 * copying to CPU. Also column wise reductions can not be used as arguments to
 * other operations - they can only be evaluated.
 * @tparam T type of input expression
 * @param a expression to reduce
 * @return min
 */
template <typename T, require_all_kernel_expressions_t<T>* = nullptr>
inline auto colwise_min(T&& a) {
  return colwise_min_<as_operation_cl_t<T>>(
      as_operation_cl(std::forward<T>(a)));
}

namespace internal {
template <typename T>
struct is_colwise_reduction_impl
    : public std::is_base_of<internal::colwise_reduction_base,
                             std::decay_t<T>> {};
template <typename T>
struct is_colwise_reduction_impl<calc_if_<true, T>>
    : public std::is_base_of<internal::colwise_reduction_base,
                             std::decay_t<T>> {};
}  // namespace internal

/**
 * Check whether a kernel generator expression is a colwise reduction.
 */
template <typename T>
using is_colwise_reduction
    = internal::is_colwise_reduction_impl<std::decay_t<T>>;

/** @}*/
}  // namespace math
}  // namespace stan
#endif
#endif
<|endoftext|>


<commit_before>#include "system/payload_system.hpp"
#include "util/time.hpp"

#include "ch.hpp"
#include "chprintf.h"

PayloadSystem::PayloadSystem(
    Accelerometer& accel,
    optional<Accelerometer *> accelH,
    optional<Barometer *> bar,
    optional<GPS *> gps,
    Gyroscope& gyr,
    optional<Magnetometer *> mag,
    WorldEstimator& estimator, InputSource& inputSource,
    MotorMapper& motorMapper, Communicator& communicator,
    Platform& platform)
  : VehicleSystem(communicator), MessageListener(communicator),
    accel(accel), accelH(accelH), bar(bar), gps(gps), gyr(gyr), mag(mag),
    estimator(estimator), inputSource(inputSource),
    motorMapper(motorMapper), platform(platform),
    imuStream(communicator, 100),
    systemStream(communicator, 5),
    state(PayloadState::DISARMED), motorDC(0.0) {
  // Disarm by default. A set_arm_state_message_t message is required to enable
  // the control pipeline.
  setArmed(false);
}

void PayloadSystem::update() {
  //static int time = 0;
  //if (time % 1000 == 0) {
  //  chprintf((BaseSequentialStream*)&SD4, "%d\r\n", RTT2MS(chibios_rt::System::getTime()));
  //}
  //time = (time+1) % 1000;

  // Poll the gyroscope and accelerometer
  AccelerometerReading accelReading = accel.readAccel();
  GyroscopeReading gyrReading = gyr.readGyro();
  optional<AccelerometerReading> accelHReading;
  optional<BarometerReading> barReading;
  optional<GPSReading> gpsReading;
  optional<MagnetometerReading> magReading;

  if (accelH) accelHReading = (*accelH)->readAccel();
  if (bar)    barReading    = (*bar)->readBar();
  if (gps)    gpsReading    = (*gps)->readGPS();
  //if (mag)    magReading    = (*mag)->readMag();

  SensorMeasurements meas {
    .accel  = std::experimental::make_optional(accelReading),
    .accelH = accelHReading,
    .bar    = barReading,
    .gps    = gpsReading,
    .gyro   = std::experimental::make_optional(gyrReading),
    .mag    = magReading
  };

  // Update the world estimate
  WorldEstimate estimate = estimator.update(meas);

  // Poll for controller input
  ControllerInput input = inputSource.read();

  // Keep moving average of acceleration
  static float accel = -1.0f;
  accel = 0.5*accel + 0.5*accelReading.axes[0];

  // Run the controllers
  //ActuatorSetpoint actuatorSp;

  // Run state machine
  switch (state) {
  case PayloadState::DISARMED:
    state = DisarmedState(meas, estimate);
    break;
  case PayloadState::PRE_ARM:
    state = PreArmState(meas, estimate);
    break;
  case PayloadState::ARMED:
    state = ArmedState(meas, estimate);
    break;
  case PayloadState::FLIGHT:
    state = FlightState(meas, estimate);
    break;
  case PayloadState::APOGEE:
    state = ApogeeState(meas, estimate);
    break;
  case PayloadState::ZERO_G:
    state = ZeroGState(meas, estimate);
    break;
  case PayloadState::DESCENT:
    state = DescentState(meas, estimate);
    break;
  case PayloadState::RECOVERY:
    state = RecoveryState(meas, estimate);
    break;
  default:
    break;
  }

  // Update streams
  updateStreams(meas, estimate);

  // Update motor outputs
  //motorMapper.run(isArmed(), actuatorSp);
}

bool PayloadSystem::healthy() {
  bool healthy = accel.healthy() && gyr.healthy();

  if(accelH) {
    healthy &= (*accelH)->healthy();
  }

  if(bar) {
    healthy &= (*bar)->healthy();
  }

  if(gps) {
    healthy &= (*gps)->healthy();
  }

  if(mag) {
    healthy &= (*mag)->healthy();
  }

  return healthy;
}

void PayloadSystem::on(const protocol::message::set_arm_state_message_t& m) {
  setArmed(m.armed);
}

void PayloadSystem::updateStreams(SensorMeasurements meas, WorldEstimate est) {
  if (imuStream.ready()) {
    protocol::message::imu_message_t m {
      .time = ST2MS(chibios_rt::System::getTime()),
      .gyro = {
        (*meas.gyro).axes[0],
        (*meas.gyro).axes[1],
        (*meas.gyro).axes[2]
      },
      .accel = {
        (*meas.accel).axes[0],
        (*meas.accel).axes[1],
        (*meas.accel).axes[2]
      }
    };

    imuStream.publish(m);
  }

  if (systemStream.ready()) {
    uint8_t stateNum = 0;
    switch (state) {
    case PayloadState::DISARMED:
      stateNum = 0;
      break;
    case PayloadState::PRE_ARM:
      stateNum = 1;
      break;
    case PayloadState::ARMED:
      stateNum = 2;
      break;
    case PayloadState::FLIGHT:
      stateNum = 3;
      break;
    case PayloadState::APOGEE:
      stateNum = 4;
      break;
    case PayloadState::ZERO_G:
      stateNum = 5;
      break;
    case PayloadState::DESCENT:
      stateNum = 6;
      break;
    case PayloadState::RECOVERY:
      stateNum = 7;
      break;
    default:
      break;
    }

    protocol::message::system_message_t m {
      .time = ST2MS(chibios_rt::System::getTime()),
      .state = stateNum,
      .motorDC = motorDC
    };

    systemStream.publish(m);
  }
}

PayloadState PayloadSystem::DisarmedState(SensorMeasurements meas, WorldEstimate est) {
  PulseLED(1,0,0,1);   // Red 1 Hz

  // Proceed directly to PRE_ARM for now.
  return PayloadState::PRE_ARM;
}

PayloadState PayloadSystem::PreArmState(SensorMeasurements meas, WorldEstimate est) {
  PulseLED(1,0,0,4);   // Red 4 Hz

  // Calibrate
  groundAltitude = est.loc.alt;
  // TODO(yoos): run sensor calibration here

  // Proceed to ARMED if all sensors are healthy and GS arm signal received.
  if (healthy() && isArmed()) {
    return PayloadState::ARMED;
  }

  return PayloadState::PRE_ARM;
}

PayloadState PayloadSystem::ArmedState(SensorMeasurements meas, WorldEstimate est) {
  SetLED(1,0,0);   // Red

  static int count = 10;
  count = ((*meas.accel).axes[0] > 1.1) ? (count-1) : 10;

  // Revert to PRE_ARM if any sensors are unhealthy or disarm signal received
  if (!(healthy() && isArmed())) {
    return PayloadState::PRE_ARM;
  }

  // Proceed to FLIGHT on 1.1g sense on X axis.
  if (count == 0) {
    return PayloadState::FLIGHT;
  }

  return PayloadState::ARMED;
}

PayloadState PayloadSystem::FlightState(SensorMeasurements meas, WorldEstimate est) {
  SetLED(0,0,1);   // Blue
  static bool powered = true;   // First time we enter, we are in powered flight.

  // Check for motor cutoff. We should see negative acceleration due to drag.
  static int count = 100;
  if (powered) {
    count = ((*meas.accel).axes[0] < 0.0) ? (count-1) : 100;
    powered = (count == 0) ? false : true;
  }

  // Apogee occurs after motor cutoff
  if (!powered) {
    // If falling faster than -40m/s, definitely deploy.
    if (est.loc.dAlt < -40.0) {
      return PayloadState::APOGEE;
    }
    // Check for near-zero altitude change towards end of ascent (ideal case)
    // and that we are not just undergoing a subsonic transition.
    else if ((*meas.accel).axes[0] > -1.0) {
      return PayloadState::APOGEE;
    }
  }

  return PayloadState::FLIGHT;
}

PayloadState PayloadSystem::ApogeeState(SensorMeasurements meas, WorldEstimate est) {
  PulseLED(0,0,1,2);   // Blue 2 Hz
  static float sTime = 0.0;   // State time

  // Count continuous time under drogue
  // TODO(yoos): We might still see this if partially deployed and spinning
  // around..
  static float drogueTime = 0.0;
  if ((*meas.accel).axes[0] < -0.3) {
    drogueTime += unit_config::DT;
  }
  else {
    drogueTime = 0.0;
  }

  // Wait until successful drogue deployment or 5 seconds max.
  if (sTime < 5.0) {
    if (drogueTime > 1.0) {   // TODO(yoos): Do we want to wait longer for ensure drogue?
      return PayloadState::ZERO_G;
    }
    // TODO: What if payload doesn't separate?
  }
  else {
    return PayloadState::ZERO_G;
  }

  sTime += unit_config::DT;
  return PayloadState::APOGEE;
}

PayloadState PayloadSystem::ZeroGState(SensorMeasurements meas, WorldEstimate est) {
  RGBLED(2);   // Rainbows!
  static float sTime = 0.0;   // State time

  // Release shuttle
  platform.get<DigitalPlatform>().set(unit_config::PIN_SHUTTLE1_CH, true);
  platform.get<DigitalPlatform>().set(unit_config::PIN_SHUTTLE2_CH, true);

  // Run zero-g maneuver for 6 s.
  if (sTime < 6.0) {
    if ((*meas.accel).axes[0] < 0.0 &&
        motorDC < 1.0) {
      motorDC += 0.01;
    }
    ActuatorSetpoint actuatorSp {0,0,0,motorDC};
    motorMapper.run(true, actuatorSp);   // Assumed armed
  }
  else if (sTime < 7.0) {
    motorDC = 0.0;
    ActuatorSetpoint actuatorSp {0,0,0,motorDC};
    motorMapper.run(true, actuatorSp);   // Assumed armed
  }
  else if (sTime < 10.0) {
    // Fire drogue pyro
    platform.get<DigitalPlatform>().set(unit_config::PIN_DROGUE_CH, true);
    if ((*meas.accel).axes[0] < -10.0) {   // Large negative acceleration due to proper drogue deployment
      return PayloadState::DESCENT;
    }
  }
  else {
    // We would deploy main here, but payload avionics doesn't have room.
    return PayloadState::DESCENT;
  }

  sTime += unit_config::DT;
  return PayloadState::ZERO_G;
}

PayloadState PayloadSystem::DescentState(SensorMeasurements meas, WorldEstimate est) {
  SetLED(1,0,1);   // Violet
  static float sTime = 0.0;   // State time

  // Payload does not control its own main.

  // Stay for at least 1 s
  static int count = 1000;
  if (sTime < 1.0) {}
  // Enter recovery if altitude is unchanging and rotation rate is zero
  else if (est.loc.dAlt > -2.0 &&
      fabs((*meas.gyro).axes[0] < 0.05) &&
      fabs((*meas.gyro).axes[1] < 0.05) &&
      fabs((*meas.gyro).axes[2] < 0.05)) {
    count -= 1;
  }
  else {
    count = 1000;
  }

  if (count == 0) {
    return PayloadState::RECOVERY;
  }

  sTime += unit_config::DT;
  return PayloadState::DESCENT;
}

PayloadState PayloadSystem::RecoveryState(SensorMeasurements meas, WorldEstimate est) {
  PulseLED(1,0,1,2);   // Violet 2 Hz

  // Turn things off
  platform.get<DigitalPlatform>().set(unit_config::PIN_DROGUE_CH, false);

  return PayloadState::RECOVERY;
}

void PayloadSystem::SetLED(float r, float g, float b) {
  platform.get<PWMPlatform>().set(9,  0.1*r);
  platform.get<PWMPlatform>().set(10, 0.1*g);
  platform.get<PWMPlatform>().set(11, 0.1*b);
}

void PayloadSystem::BlinkLED(float r, float g, float b, float freq) { static int count = 0;
  int period = 1000 / freq;
  if (count % period < period/2) {
    SetLED(r,g,b);
  }
  else {
    SetLED(0,0,0);
  }
  count = (count+1) % period;
}

void PayloadSystem::PulseLED(float r, float g, float b, float freq) {
  int period = 1000 / freq;
  static int count = 0;
  float dc = ((float) abs(period/2 - count)) / (period/2);
  SetLED(dc*r, dc*g, dc*b);
  count = (count+1) % period;
}

void PayloadSystem::RGBLED(float freq) {
  static float dc = 0.0;
  static int dir = 1;
  if (dc >= 1.0) {
    dir = -1;
  }
  else if (dc <= 0.0) {
    dir = 1;
  }
  dc += dir * (2*freq * unit_config::DT);

  float dc_ = dc;
  float dir_ = dir;
  for (int i=0; i<3; i++) {
    dc_ += dir_ * 0.666;
    if (dc_ > 1.0) {
      dc_ = 2.0 - dc_;
      dir_ = -1;
    }
    else if (dc_ < 0.0) {
      dc_ = 0.0 - dc_;
      dir_ = 1;
    }
    platform.get<PWMPlatform>().set(9+i, dc_);
  }
}
<commit_msg>Decrease payload IMU stream frequency.<commit_after>#include "system/payload_system.hpp"
#include "util/time.hpp"

#include "ch.hpp"
#include "chprintf.h"

PayloadSystem::PayloadSystem(
    Accelerometer& accel,
    optional<Accelerometer *> accelH,
    optional<Barometer *> bar,
    optional<GPS *> gps,
    Gyroscope& gyr,
    optional<Magnetometer *> mag,
    WorldEstimator& estimator, InputSource& inputSource,
    MotorMapper& motorMapper, Communicator& communicator,
    Platform& platform)
  : VehicleSystem(communicator), MessageListener(communicator),
    accel(accel), accelH(accelH), bar(bar), gps(gps), gyr(gyr), mag(mag),
    estimator(estimator), inputSource(inputSource),
    motorMapper(motorMapper), platform(platform),
    imuStream(communicator, 10),   // TODO(yoos): calculate data link budget and increase if possible
    systemStream(communicator, 5),
    state(PayloadState::DISARMED), motorDC(0.0) {
  // Disarm by default. A set_arm_state_message_t message is required to enable
  // the control pipeline.
  setArmed(false);
}

void PayloadSystem::update() {
  //static int time = 0;
  //if (time % 1000 == 0) {
  //  chprintf((BaseSequentialStream*)&SD4, "%d\r\n", RTT2MS(chibios_rt::System::getTime()));
  //}
  //time = (time+1) % 1000;

  // Poll the gyroscope and accelerometer
  AccelerometerReading accelReading = accel.readAccel();
  GyroscopeReading gyrReading = gyr.readGyro();
  optional<AccelerometerReading> accelHReading;
  optional<BarometerReading> barReading;
  optional<GPSReading> gpsReading;
  optional<MagnetometerReading> magReading;

  if (accelH) accelHReading = (*accelH)->readAccel();
  if (bar)    barReading    = (*bar)->readBar();
  if (gps)    gpsReading    = (*gps)->readGPS();
  //if (mag)    magReading    = (*mag)->readMag();

  SensorMeasurements meas {
    .accel  = std::experimental::make_optional(accelReading),
    .accelH = accelHReading,
    .bar    = barReading,
    .gps    = gpsReading,
    .gyro   = std::experimental::make_optional(gyrReading),
    .mag    = magReading
  };

  // Update the world estimate
  WorldEstimate estimate = estimator.update(meas);

  // Poll for controller input
  ControllerInput input = inputSource.read();

  // Keep moving average of acceleration
  static float accel = -1.0f;
  accel = 0.5*accel + 0.5*accelReading.axes[0];

  // Run the controllers
  //ActuatorSetpoint actuatorSp;

  // Run state machine
  switch (state) {
  case PayloadState::DISARMED:
    state = DisarmedState(meas, estimate);
    break;
  case PayloadState::PRE_ARM:
    state = PreArmState(meas, estimate);
    break;
  case PayloadState::ARMED:
    state = ArmedState(meas, estimate);
    break;
  case PayloadState::FLIGHT:
    state = FlightState(meas, estimate);
    break;
  case PayloadState::APOGEE:
    state = ApogeeState(meas, estimate);
    break;
  case PayloadState::ZERO_G:
    state = ZeroGState(meas, estimate);
    break;
  case PayloadState::DESCENT:
    state = DescentState(meas, estimate);
    break;
  case PayloadState::RECOVERY:
    state = RecoveryState(meas, estimate);
    break;
  default:
    break;
  }

  // Update streams
  updateStreams(meas, estimate);

  // Update motor outputs
  //motorMapper.run(isArmed(), actuatorSp);
}

bool PayloadSystem::healthy() {
  bool healthy = accel.healthy() && gyr.healthy();

  if(accelH) {
    healthy &= (*accelH)->healthy();
  }

  if(bar) {
    healthy &= (*bar)->healthy();
  }

  if(gps) {
    healthy &= (*gps)->healthy();
  }

  if(mag) {
    healthy &= (*mag)->healthy();
  }

  return healthy;
}

void PayloadSystem::on(const protocol::message::set_arm_state_message_t& m) {
  setArmed(m.armed);
}

void PayloadSystem::updateStreams(SensorMeasurements meas, WorldEstimate est) {
  if (imuStream.ready()) {
    protocol::message::imu_message_t m {
      .time = ST2MS(chibios_rt::System::getTime()),
      .gyro = {
        (*meas.gyro).axes[0],
        (*meas.gyro).axes[1],
        (*meas.gyro).axes[2]
      },
      .accel = {
        (*meas.accel).axes[0],
        (*meas.accel).axes[1],
        (*meas.accel).axes[2]
      }
    };

    imuStream.publish(m);
  }

  if (systemStream.ready()) {
    uint8_t stateNum = 0;
    switch (state) {
    case PayloadState::DISARMED:
      stateNum = 0;
      break;
    case PayloadState::PRE_ARM:
      stateNum = 1;
      break;
    case PayloadState::ARMED:
      stateNum = 2;
      break;
    case PayloadState::FLIGHT:
      stateNum = 3;
      break;
    case PayloadState::APOGEE:
      stateNum = 4;
      break;
    case PayloadState::ZERO_G:
      stateNum = 5;
      break;
    case PayloadState::DESCENT:
      stateNum = 6;
      break;
    case PayloadState::RECOVERY:
      stateNum = 7;
      break;
    default:
      break;
    }

    protocol::message::system_message_t m {
      .time = ST2MS(chibios_rt::System::getTime()),
      .state = stateNum,
      .motorDC = motorDC
    };

    systemStream.publish(m);
  }
}

PayloadState PayloadSystem::DisarmedState(SensorMeasurements meas, WorldEstimate est) {
  PulseLED(1,0,0,1);   // Red 1 Hz

  // Proceed directly to PRE_ARM for now.
  return PayloadState::PRE_ARM;
}

PayloadState PayloadSystem::PreArmState(SensorMeasurements meas, WorldEstimate est) {
  PulseLED(1,0,0,4);   // Red 4 Hz

  // Calibrate
  groundAltitude = est.loc.alt;
  // TODO(yoos): run sensor calibration here

  // Proceed to ARMED if all sensors are healthy and GS arm signal received.
  if (healthy() && isArmed()) {
    return PayloadState::ARMED;
  }

  return PayloadState::PRE_ARM;
}

PayloadState PayloadSystem::ArmedState(SensorMeasurements meas, WorldEstimate est) {
  SetLED(1,0,0);   // Red

  static int count = 10;
  count = ((*meas.accel).axes[0] > 1.1) ? (count-1) : 10;

  // Revert to PRE_ARM if any sensors are unhealthy or disarm signal received
  if (!(healthy() && isArmed())) {
    return PayloadState::PRE_ARM;
  }

  // Proceed to FLIGHT on 1.1g sense on X axis.
  if (count == 0) {
    return PayloadState::FLIGHT;
  }

  return PayloadState::ARMED;
}

PayloadState PayloadSystem::FlightState(SensorMeasurements meas, WorldEstimate est) {
  SetLED(0,0,1);   // Blue
  static bool powered = true;   // First time we enter, we are in powered flight.

  // Check for motor cutoff. We should see negative acceleration due to drag.
  static int count = 100;
  if (powered) {
    count = ((*meas.accel).axes[0] < 0.0) ? (count-1) : 100;
    powered = (count == 0) ? false : true;
  }

  // Apogee occurs after motor cutoff
  if (!powered) {
    // If falling faster than -40m/s, definitely deploy.
    if (est.loc.dAlt < -40.0) {
      return PayloadState::APOGEE;
    }
    // Check for near-zero altitude change towards end of ascent (ideal case)
    // and that we are not just undergoing a subsonic transition.
    else if ((*meas.accel).axes[0] > -1.0) {
      return PayloadState::APOGEE;
    }
  }

  return PayloadState::FLIGHT;
}

PayloadState PayloadSystem::ApogeeState(SensorMeasurements meas, WorldEstimate est) {
  PulseLED(0,0,1,2);   // Blue 2 Hz
  static float sTime = 0.0;   // State time

  // Count continuous time under drogue
  // TODO(yoos): We might still see this if partially deployed and spinning
  // around..
  static float drogueTime = 0.0;
  if ((*meas.accel).axes[0] < -0.3) {
    drogueTime += unit_config::DT;
  }
  else {
    drogueTime = 0.0;
  }

  // Wait until successful drogue deployment or 5 seconds max.
  if (sTime < 5.0) {
    if (drogueTime > 1.0) {   // TODO(yoos): Do we want to wait longer for ensure drogue?
      return PayloadState::ZERO_G;
    }
    // TODO: What if payload doesn't separate?
  }
  else {
    return PayloadState::ZERO_G;
  }

  sTime += unit_config::DT;
  return PayloadState::APOGEE;
}

PayloadState PayloadSystem::ZeroGState(SensorMeasurements meas, WorldEstimate est) {
  RGBLED(2);   // Rainbows!
  static float sTime = 0.0;   // State time

  // Release shuttle
  platform.get<DigitalPlatform>().set(unit_config::PIN_SHUTTLE1_CH, true);
  platform.get<DigitalPlatform>().set(unit_config::PIN_SHUTTLE2_CH, true);

  // Run zero-g maneuver for 6 s.
  if (sTime < 6.0) {
    if ((*meas.accel).axes[0] < 0.0 &&
        motorDC < 1.0) {
      motorDC += 0.01;
    }
    ActuatorSetpoint actuatorSp {0,0,0,motorDC};
    motorMapper.run(true, actuatorSp);   // Assumed armed
  }
  else if (sTime < 7.0) {
    motorDC = 0.0;
    ActuatorSetpoint actuatorSp {0,0,0,motorDC};
    motorMapper.run(true, actuatorSp);   // Assumed armed
  }
  else if (sTime < 10.0) {
    // Fire drogue pyro
    platform.get<DigitalPlatform>().set(unit_config::PIN_DROGUE_CH, true);
    if ((*meas.accel).axes[0] < -10.0) {   // Large negative acceleration due to proper drogue deployment
      return PayloadState::DESCENT;
    }
  }
  else {
    // We would deploy main here, but payload avionics doesn't have room.
    return PayloadState::DESCENT;
  }

  sTime += unit_config::DT;
  return PayloadState::ZERO_G;
}

PayloadState PayloadSystem::DescentState(SensorMeasurements meas, WorldEstimate est) {
  SetLED(1,0,1);   // Violet
  static float sTime = 0.0;   // State time

  // Payload does not control its own main.

  // Stay for at least 1 s
  static int count = 1000;
  if (sTime < 1.0) {}
  // Enter recovery if altitude is unchanging and rotation rate is zero
  else if (est.loc.dAlt > -2.0 &&
      fabs((*meas.gyro).axes[0] < 0.05) &&
      fabs((*meas.gyro).axes[1] < 0.05) &&
      fabs((*meas.gyro).axes[2] < 0.05)) {
    count -= 1;
  }
  else {
    count = 1000;
  }

  if (count == 0) {
    return PayloadState::RECOVERY;
  }

  sTime += unit_config::DT;
  return PayloadState::DESCENT;
}

PayloadState PayloadSystem::RecoveryState(SensorMeasurements meas, WorldEstimate est) {
  PulseLED(1,0,1,2);   // Violet 2 Hz

  // Turn things off
  platform.get<DigitalPlatform>().set(unit_config::PIN_DROGUE_CH, false);

  return PayloadState::RECOVERY;
}

void PayloadSystem::SetLED(float r, float g, float b) {
  platform.get<PWMPlatform>().set(9,  0.1*r);
  platform.get<PWMPlatform>().set(10, 0.1*g);
  platform.get<PWMPlatform>().set(11, 0.1*b);
}

void PayloadSystem::BlinkLED(float r, float g, float b, float freq) { static int count = 0;
  int period = 1000 / freq;
  if (count % period < period/2) {
    SetLED(r,g,b);
  }
  else {
    SetLED(0,0,0);
  }
  count = (count+1) % period;
}

void PayloadSystem::PulseLED(float r, float g, float b, float freq) {
  int period = 1000 / freq;
  static int count = 0;
  float dc = ((float) abs(period/2 - count)) / (period/2);
  SetLED(dc*r, dc*g, dc*b);
  count = (count+1) % period;
}

void PayloadSystem::RGBLED(float freq) {
  static float dc = 0.0;
  static int dir = 1;
  if (dc >= 1.0) {
    dir = -1;
  }
  else if (dc <= 0.0) {
    dir = 1;
  }
  dc += dir * (2*freq * unit_config::DT);

  float dc_ = dc;
  float dir_ = dir;
  for (int i=0; i<3; i++) {
    dc_ += dir_ * 0.666;
    if (dc_ > 1.0) {
      dc_ = 2.0 - dc_;
      dir_ = -1;
    }
    else if (dc_ < 0.0) {
      dc_ = 0.0 - dc_;
      dir_ = 1;
    }
    platform.get<PWMPlatform>().set(9+i, dc_);
  }
}
<|endoftext|>


<commit_before>#include "TerrainMap.hpp"

#include <limits>

#include <lcm/lcm-cpp.hpp>
#include <lcmtypes/bot_core/pose_t.hpp>

#include <drc_utils/BotWrapper.hpp>

#include <maps/BotWrapper.hpp>
#include <maps/ViewClient.hpp>
#include <maps/DepthImage.hpp>
#include <maps/DepthImageView.hpp>

#include "FillMethods.hpp"

using namespace terrainmap;

struct TerrainMap::Helper {

  struct Listener : public maps::ViewClient::Listener {
    Helper* mHelper;
    Listener(Helper* iHelper) : mHelper(iHelper) {}
    void notifyCatalog(const bool iChanged) {}
    void notifyData(const int64_t iViewId) {
      if (!mHelper->mListening) return;
      if (iViewId != mHelper->mViewId) return;
      auto view = std::dynamic_pointer_cast<maps::DepthImageView>
        (mHelper->mViewClient->getView(mHelper->mViewId));
      if (view != NULL) {
        auto type = maps::DepthImage::TypeDisparity;
        mHelper->mPreviousDepths = view->getDepthImage()->getData(type);
      }
      mHelper->mLatestView = view;
      mHelper->computeNewDepths();
    }
  };

  int64_t mViewId;
  std::string mMapChannel;
  Eigen::Vector4d mFillPlane;
  bool mUseFootPose;
  bool mOverrideHeights;
  bool mShouldFillMissing;
  NormalMethod mNormalMethod;
  double mNormalRadius;
  std::shared_ptr<drc::BotWrapper> mBotWrapper;

  std::shared_ptr<maps::ViewClient> mViewClient;
  std::shared_ptr<Listener> mListener;
  std::shared_ptr<maps::DepthImageView> mLatestView;
  Eigen::Vector4d mLatestFootPlane;
  std::vector<float> mPreviousDepths;
  bool mListening;

  Helper(const std::shared_ptr<drc::BotWrapper>& iBotWrapper) {
    mBotWrapper = iBotWrapper;
    mUseFootPose = false;
    mOverrideHeights = false;
    mNormalMethod = NormalMethodTriangle;
    mShouldFillMissing = true;
    mNormalRadius = 0;
    mListening = false;
    mFillPlane << 0,0,1,0;
    mLatestFootPlane << 0,0,1,0;
    mViewClient.reset(new maps::ViewClient());
    std::shared_ptr<maps::BotWrapper> wrapper
      (new maps::BotWrapper(mBotWrapper->getLcm(), mBotWrapper->getBotParam(),
                            mBotWrapper->getBotFrames()));
    mViewClient->setBotWrapper(wrapper);
    mBotWrapper->getLcm()->subscribe("POSE_GROUND", &Helper::onGround, this);
    mListener.reset(new Listener(this));
    mViewClient->addListener(mListener.get());
  }

  // TODO: need this? could also use bot-frames from ground to local
  // need to make sure that info is synchronized even when listening paused
  void onGround(const lcm::ReceiveBuffer* iBuf,
                const std::string& iChannel,
                const bot_core::pose_t* iMessage) {
    if (!mListening) return;
    Eigen::Quaterniond q(iMessage->orientation[0], iMessage->orientation[1],
                         iMessage->orientation[2], iMessage->orientation[3]);
    Eigen::Vector3d pos(iMessage->pos[0], iMessage->pos[1], iMessage->pos[2]);
    mLatestFootPlane.head<3>() = q.matrix().col(2);
    mLatestFootPlane[3] = -mLatestFootPlane.head<3>().dot(pos);
  }

  Eigen::Vector4d getLatestFootPlane() const {
    // TODO: below might be slow if called too frequently
    if (false) {
      Eigen::Isometry3d groundPose;
      mBotWrapper->getTransform("ground","local",groundPose);
      Eigen::Vector4d plane;
      plane.head<3>() = groundPose.rotation().col(2);
      plane[3] = -plane.head<3>().dot(groundPose.translation());
      return plane;
    }

    return mLatestFootPlane;
  }

  void computeNewDepths() {
    auto view = mLatestView;
    auto plane = mUseFootPose ? getLatestFootPlane() : mFillPlane;
    if (mOverrideHeights) {
      FillMethods::fillEntireView(view, plane);
    }
    else {
      if (mShouldFillMissing) {
        // fill 0.5m square area underneath robot
        // TODO: could use timestamp of latest view
        Eigen::Isometry3d groundToLocal;
        mBotWrapper->getTransform("ground","local",groundToLocal);
        Eigen::Vector3d pos = groundToLocal.translation();
        FillMethods::fillBox(view, pos, 0.5, plane);

        // fill holes in environment using neighbor values
        FillMethods::fillHolesIterative(view);

        // fill remaining areas with selected plane
        FillMethods::fillMissing(view, plane);
      }
    }
  }

};

TerrainMap::
TerrainMap(const std::shared_ptr<drc::BotWrapper>& iBotWrapper) {
  mHelper.reset(new Helper(iBotWrapper));
}

TerrainMap::
~TerrainMap() {
}

void TerrainMap::
setInfo(const int64_t iViewId, const std::string& iMapChannel) {
  mHelper->mViewId = iViewId;
  mHelper->mMapChannel = iMapChannel;
  mHelper->mViewClient->removeAllViewChannels();
  mHelper->mViewClient->addViewChannel(iMapChannel);
}

int64_t TerrainMap::
getViewId() const {
  return mHelper->mViewId;
}

std::string TerrainMap::
getMapChannel() const {
  return mHelper->mMapChannel;
}

void TerrainMap::
setFillPlane(const Eigen::Vector4d& iPlane) {
  mHelper->mFillPlane = iPlane;
  mHelper->mFillPlane /= iPlane.head<3>().norm();
}

void TerrainMap::
useFootPose(const bool iVal) {
  mHelper->mUseFootPose = iVal;
}

void TerrainMap::
overrideHeights(const bool iVal) {
  mHelper->mOverrideHeights = iVal;
}

void TerrainMap::
setNormalMethod(const NormalMethod iMethod) {
  mHelper->mNormalMethod = iMethod;
}

void TerrainMap::
setNormalRadius(const double iRadius) {
  mHelper->mNormalRadius = iRadius;
}

void TerrainMap::
shouldFillMissing(const bool iVal) {
  mHelper->mShouldFillMissing = iVal;
}

double TerrainMap::
getNormalRadius() const {
  return mHelper->mNormalRadius;
}

bool TerrainMap::
startListening() {
  if (mHelper->mListening) return false;
  mHelper->mViewClient->start();
  mHelper->mListening = true;
  return true;
}

bool TerrainMap::
stopListening() {
  if (!mHelper->mListening) return false;
  mHelper->mListening = false;
  mHelper->mViewClient->stop();
  return true;
}

bool TerrainMap::
sendRequest(const Eigen::Vector3d& iBoxMin, const Eigen::Vector3d& iBoxMax,
            const double iResolutionMeters, const double iTimeWindowSeconds,
            const double iFrequencyHz) {

  maps::ViewBase::Spec spec;
  spec.mResolution = (float)iResolutionMeters;
  spec.mWidth = int((iBoxMax[0] - iBoxMin[0]) / spec.mResolution);
  spec.mHeight = int((iBoxMax[1] - iBoxMin[1]) / spec.mResolution);
  spec.mTimeMin = -iTimeWindowSeconds*1e6;
  spec.mClipPlanes.push_back(Eigen::Vector4f( 1, 0, 0, -iBoxMin[0]));
  spec.mClipPlanes.push_back(Eigen::Vector4f(-1, 0, 0,  iBoxMax[0]));
  spec.mClipPlanes.push_back(Eigen::Vector4f( 0, 1, 0, -iBoxMin[1]));
  spec.mClipPlanes.push_back(Eigen::Vector4f( 0,-1, 0,  iBoxMax[1]));
  spec.mClipPlanes.push_back(Eigen::Vector4f( 0, 0, 1, -iBoxMin[2]));
  spec.mClipPlanes.push_back(Eigen::Vector4f( 0, 0,-1,  iBoxMax[2]));

  spec.mMapId = 1;
  spec.mViewId = getViewId();
  spec.mType = maps::ViewBase::TypeDepthImage;
  spec.mChannel = getMapChannel();
  spec.mFrequency = iFrequencyHz;
  spec.mQuantizationMax = 0;
  spec.mTimeMax = 0;
  spec.mTimeMode = maps::ViewBase::TimeModeRelative;
  spec.mRelativeLocation = true;
  spec.mActive = true;
  Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
  pose.translation() = Eigen::Vector3f(0,0,10);
  pose.linear() << 1,0,0, 0,-1,0, 0,0,-1;
  Eigen::Affine3f calib = Eigen::Affine3f::Identity();
  calib(0,0) = 1/spec.mResolution;
  calib(1,1) = 1/spec.mResolution;
  calib(0,3) = -iBoxMin[0]*calib(0,0);
  calib(1,3) = -iBoxMin[1]*calib(1,1);
  Eigen::Projective3f projector = calib*pose.inverse();
  spec.mTransform = projector;

  // send request
  mHelper->mViewClient->request(spec);

  return true;
}

std::shared_ptr<TerrainMap::TerrainData> TerrainMap::
getData() const {
  auto view = mHelper->mLatestView;
  if (view == NULL) return NULL;
  std::shared_ptr<TerrainData> data(new TerrainData());
  auto type = maps::DepthImage::TypeDepth;
  auto image = view->getDepthImage();
  data->mHeights = image->getData(type);
  data->mWidth = image->getWidth();
  data->mHeight = image->getHeight();
  data->mTransform = view->getTransform().cast<double>();
  return data;
}

template <typename T>
bool TerrainMap::
getHeightAndNormal(const T iX, const T iY,
                   T& oHeight, Eigen::Matrix<T,3,1>& oNormal) const {
  bool usePlaneForHeight = mHelper->mOverrideHeights;
  bool usePlaneForNormal = (mHelper->mNormalMethod == NormalMethodOverride);
  if (!(usePlaneForHeight && usePlaneForNormal)) {
    Eigen::Vector3f pt, normal;
    const auto& view = mHelper->mLatestView;
    auto normalMethod = maps::DepthImageView::NormalMethodTriangle;
    switch (mHelper->mNormalMethod) {
    case NormalMethodOverride:
      normalMethod = maps::DepthImageView::NormalMethodZ; break;
    case NormalMethodTriangle:
      normalMethod = maps::DepthImageView::NormalMethodTriangle; break;
    case NormalMethodLeastSquares:
      normalMethod = maps::DepthImageView::NormalMethodLeastSquares; break;
    case NormalMethodRobustKernel:
      normalMethod = maps::DepthImageView::NormalMethodRobustKernel; break;
    case NormalMethodSampleConsensus:
      normalMethod = maps::DepthImageView::NormalMethodSampleConsensus; break;
    default: break;
    }
    if (view == NULL) usePlaneForHeight = usePlaneForNormal = true;
    else {
      view->setNormalMethod(normalMethod);
      if (!view->getClosest(Eigen::Vector3f(iX, iY, 0), pt, normal)) {
        usePlaneForHeight = usePlaneForNormal = true;
      }
    }
    oHeight = pt[2];
    oNormal = normal.cast<T>();
  }

  const auto& plane = (mHelper->mUseFootPose ? mHelper->getLatestFootPlane() :
                       mHelper->mFillPlane).cast<T>();
  if (usePlaneForHeight) {
    oHeight = -(plane[0]*iX + plane[1]*iY + plane[3])/plane[2];
  }
  if (usePlaneForNormal) {
    oNormal << plane[0],plane[1],plane[2];
  }
  return true;
}

// explicit instantiations
template bool TerrainMap::
getHeightAndNormal(const float iX, const float iY,
                   float& oHeight, Eigen::Vector3f& oNormal) const;
template bool TerrainMap::
getHeightAndNormal(const double iX, const double iY,
                   double& oHeight, Eigen::Vector3d& oNormal) const;
<commit_msg>fixed bug in terrainmap; reference was being assigned to temporary<commit_after>#include "TerrainMap.hpp"

#include <limits>

#include <lcm/lcm-cpp.hpp>
#include <lcmtypes/bot_core/pose_t.hpp>

#include <drc_utils/BotWrapper.hpp>

#include <maps/BotWrapper.hpp>
#include <maps/ViewClient.hpp>
#include <maps/DepthImage.hpp>
#include <maps/DepthImageView.hpp>

#include "FillMethods.hpp"

using namespace terrainmap;

struct TerrainMap::Helper {

  struct Listener : public maps::ViewClient::Listener {
    Helper* mHelper;
    Listener(Helper* iHelper) : mHelper(iHelper) {}
    void notifyCatalog(const bool iChanged) {}
    void notifyData(const int64_t iViewId) {
      if (!mHelper->mListening) return;
      if (iViewId != mHelper->mViewId) return;
      auto view = std::dynamic_pointer_cast<maps::DepthImageView>
        (mHelper->mViewClient->getView(mHelper->mViewId));
      if (view != NULL) {
        auto type = maps::DepthImage::TypeDisparity;
        mHelper->mPreviousDepths = view->getDepthImage()->getData(type);
      }
      mHelper->mLatestView = view;
      mHelper->computeNewDepths();
    }
  };

  int64_t mViewId;
  std::string mMapChannel;
  Eigen::Vector4d mFillPlane;
  bool mUseFootPose;
  bool mOverrideHeights;
  bool mShouldFillMissing;
  NormalMethod mNormalMethod;
  double mNormalRadius;
  std::shared_ptr<drc::BotWrapper> mBotWrapper;

  std::shared_ptr<maps::ViewClient> mViewClient;
  std::shared_ptr<Listener> mListener;
  std::shared_ptr<maps::DepthImageView> mLatestView;
  Eigen::Vector4d mLatestFootPlane;
  std::vector<float> mPreviousDepths;
  bool mListening;

  Helper(const std::shared_ptr<drc::BotWrapper>& iBotWrapper) {
    mBotWrapper = iBotWrapper;
    mUseFootPose = false;
    mOverrideHeights = false;
    mNormalMethod = NormalMethodTriangle;
    mShouldFillMissing = true;
    mNormalRadius = 0;
    mListening = false;
    mFillPlane << 0,0,1,0;
    mLatestFootPlane << 0,0,1,0;
    mViewClient.reset(new maps::ViewClient());
    std::shared_ptr<maps::BotWrapper> wrapper
      (new maps::BotWrapper(mBotWrapper->getLcm(), mBotWrapper->getBotParam(),
                            mBotWrapper->getBotFrames()));
    mViewClient->setBotWrapper(wrapper);
    mBotWrapper->getLcm()->subscribe("POSE_GROUND", &Helper::onGround, this);
    mListener.reset(new Listener(this));
    mViewClient->addListener(mListener.get());
  }

  // TODO: need this? could also use bot-frames from ground to local
  // need to make sure that info is synchronized even when listening paused
  void onGround(const lcm::ReceiveBuffer* iBuf,
                const std::string& iChannel,
                const bot_core::pose_t* iMessage) {
    if (!mListening) return;
    Eigen::Quaterniond q(iMessage->orientation[0], iMessage->orientation[1],
                         iMessage->orientation[2], iMessage->orientation[3]);
    Eigen::Vector3d pos(iMessage->pos[0], iMessage->pos[1], iMessage->pos[2]);
    mLatestFootPlane.head<3>() = q.matrix().col(2);
    mLatestFootPlane[3] = -mLatestFootPlane.head<3>().dot(pos);
  }

  Eigen::Vector4d getLatestFootPlane() const {
    // TODO: below might be slow if called too frequently
    if (false) {
      Eigen::Isometry3d groundPose;
      mBotWrapper->getTransform("ground","local",groundPose);
      Eigen::Vector4d plane;
      plane.head<3>() = groundPose.rotation().col(2);
      plane[3] = -plane.head<3>().dot(groundPose.translation());
      return plane;
    }

    return mLatestFootPlane;
  }

  void computeNewDepths() {
    auto view = mLatestView;
    auto plane = mUseFootPose ? getLatestFootPlane() : mFillPlane;
    if (mOverrideHeights) {
      FillMethods::fillEntireView(view, plane);
    }
    else {
      if (mShouldFillMissing) {
        // fill 0.5m square area underneath robot
        // TODO: could use timestamp of latest view
        Eigen::Isometry3d groundToLocal;
        mBotWrapper->getTransform("ground","local",groundToLocal);
        Eigen::Vector3d pos = groundToLocal.translation();
        FillMethods::fillBox(view, pos, 0.5, plane);

        // fill holes in environment using neighbor values
        FillMethods::fillHolesIterative(view);

        // fill remaining areas with selected plane
        FillMethods::fillMissing(view, plane);
      }
    }
  }

};

TerrainMap::
TerrainMap(const std::shared_ptr<drc::BotWrapper>& iBotWrapper) {
  mHelper.reset(new Helper(iBotWrapper));
}

TerrainMap::
~TerrainMap() {
}

void TerrainMap::
setInfo(const int64_t iViewId, const std::string& iMapChannel) {
  mHelper->mViewId = iViewId;
  mHelper->mMapChannel = iMapChannel;
  mHelper->mViewClient->removeAllViewChannels();
  mHelper->mViewClient->addViewChannel(iMapChannel);
}

int64_t TerrainMap::
getViewId() const {
  return mHelper->mViewId;
}

std::string TerrainMap::
getMapChannel() const {
  return mHelper->mMapChannel;
}

void TerrainMap::
setFillPlane(const Eigen::Vector4d& iPlane) {
  mHelper->mFillPlane = iPlane;
  mHelper->mFillPlane /= iPlane.head<3>().norm();
}

void TerrainMap::
useFootPose(const bool iVal) {
  mHelper->mUseFootPose = iVal;
}

void TerrainMap::
overrideHeights(const bool iVal) {
  mHelper->mOverrideHeights = iVal;
}

void TerrainMap::
setNormalMethod(const NormalMethod iMethod) {
  mHelper->mNormalMethod = iMethod;
}

void TerrainMap::
setNormalRadius(const double iRadius) {
  mHelper->mNormalRadius = iRadius;
}

void TerrainMap::
shouldFillMissing(const bool iVal) {
  mHelper->mShouldFillMissing = iVal;
}

double TerrainMap::
getNormalRadius() const {
  return mHelper->mNormalRadius;
}

bool TerrainMap::
startListening() {
  if (mHelper->mListening) return false;
  mHelper->mViewClient->start();
  mHelper->mListening = true;
  return true;
}

bool TerrainMap::
stopListening() {
  if (!mHelper->mListening) return false;
  mHelper->mListening = false;
  mHelper->mViewClient->stop();
  return true;
}

bool TerrainMap::
sendRequest(const Eigen::Vector3d& iBoxMin, const Eigen::Vector3d& iBoxMax,
            const double iResolutionMeters, const double iTimeWindowSeconds,
            const double iFrequencyHz) {

  maps::ViewBase::Spec spec;
  spec.mResolution = (float)iResolutionMeters;
  spec.mWidth = int((iBoxMax[0] - iBoxMin[0]) / spec.mResolution);
  spec.mHeight = int((iBoxMax[1] - iBoxMin[1]) / spec.mResolution);
  spec.mTimeMin = -iTimeWindowSeconds*1e6;
  spec.mClipPlanes.push_back(Eigen::Vector4f( 1, 0, 0, -iBoxMin[0]));
  spec.mClipPlanes.push_back(Eigen::Vector4f(-1, 0, 0,  iBoxMax[0]));
  spec.mClipPlanes.push_back(Eigen::Vector4f( 0, 1, 0, -iBoxMin[1]));
  spec.mClipPlanes.push_back(Eigen::Vector4f( 0,-1, 0,  iBoxMax[1]));
  spec.mClipPlanes.push_back(Eigen::Vector4f( 0, 0, 1, -iBoxMin[2]));
  spec.mClipPlanes.push_back(Eigen::Vector4f( 0, 0,-1,  iBoxMax[2]));

  spec.mMapId = 1;
  spec.mViewId = getViewId();
  spec.mType = maps::ViewBase::TypeDepthImage;
  spec.mChannel = getMapChannel();
  spec.mFrequency = iFrequencyHz;
  spec.mQuantizationMax = 0;
  spec.mTimeMax = 0;
  spec.mTimeMode = maps::ViewBase::TimeModeRelative;
  spec.mRelativeLocation = true;
  spec.mActive = true;
  Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
  pose.translation() = Eigen::Vector3f(0,0,10);
  pose.linear() << 1,0,0, 0,-1,0, 0,0,-1;
  Eigen::Affine3f calib = Eigen::Affine3f::Identity();
  calib(0,0) = 1/spec.mResolution;
  calib(1,1) = 1/spec.mResolution;
  calib(0,3) = -iBoxMin[0]*calib(0,0);
  calib(1,3) = -iBoxMin[1]*calib(1,1);
  Eigen::Projective3f projector = calib*pose.inverse();
  spec.mTransform = projector;

  // send request
  mHelper->mViewClient->request(spec);

  return true;
}

std::shared_ptr<TerrainMap::TerrainData> TerrainMap::
getData() const {
  auto view = mHelper->mLatestView;
  if (view == NULL) return NULL;
  std::shared_ptr<TerrainData> data(new TerrainData());
  auto type = maps::DepthImage::TypeDepth;
  auto image = view->getDepthImage();
  data->mHeights = image->getData(type);
  data->mWidth = image->getWidth();
  data->mHeight = image->getHeight();
  data->mTransform = view->getTransform().cast<double>();
  return data;
}

template <typename T>
bool TerrainMap::
getHeightAndNormal(const T iX, const T iY,
                   T& oHeight, Eigen::Matrix<T,3,1>& oNormal) const {
  bool usePlaneForHeight = mHelper->mOverrideHeights;
  bool usePlaneForNormal = (mHelper->mNormalMethod == NormalMethodOverride);
  if (!(usePlaneForHeight && usePlaneForNormal)) {
    Eigen::Vector3f pt, normal;
    const auto& view = mHelper->mLatestView;
    auto normalMethod = maps::DepthImageView::NormalMethodTriangle;
    switch (mHelper->mNormalMethod) {
    case NormalMethodOverride:
      normalMethod = maps::DepthImageView::NormalMethodZ; break;
    case NormalMethodTriangle:
      normalMethod = maps::DepthImageView::NormalMethodTriangle; break;
    case NormalMethodLeastSquares:
      normalMethod = maps::DepthImageView::NormalMethodLeastSquares; break;
    case NormalMethodRobustKernel:
      normalMethod = maps::DepthImageView::NormalMethodRobustKernel; break;
    case NormalMethodSampleConsensus:
      normalMethod = maps::DepthImageView::NormalMethodSampleConsensus; break;
    default: break;
    }
    if (view == NULL) usePlaneForHeight = usePlaneForNormal = true;
    else {
      view->setNormalMethod(normalMethod);
      if (!view->getClosest(Eigen::Vector3f(iX, iY, 0), pt, normal)) {
        usePlaneForHeight = usePlaneForNormal = true;
      }
    }
    oHeight = pt[2];
    oNormal = normal.cast<T>();
  }

  const auto plane = (mHelper->mUseFootPose ? mHelper->getLatestFootPlane() :
                      mHelper->mFillPlane).cast<T>();
  if (usePlaneForHeight) {
    oHeight = -(plane[0]*iX + plane[1]*iY + plane[3])/plane[2];
  }
  if (usePlaneForNormal) {
    oNormal << plane[0],plane[1],plane[2];
  }
  return true;
}

// explicit instantiations
template bool TerrainMap::
getHeightAndNormal(const float iX, const float iY,
                   float& oHeight, Eigen::Vector3f& oNormal) const;
template bool TerrainMap::
getHeightAndNormal(const double iX, const double iY,
                   double& oHeight, Eigen::Vector3d& oNormal) const;
<|endoftext|>


<commit_before>/*
 * This program is free software; you can redistribute it and/or modify it under
 * the terms of the GNU General Public License as published by the Free Software
 * Foundation; version 3 of the License.
 *
 * This program 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 General Public License for more
 * details.
 */

#include "../dbmanager.h"
#include "../iconmanager.h"
#include "tablewidget.h"

TableWidget::TableWidget(QWidget *parent)
  : AbstractTabWidget(parent)
{
  setupUi(this);

  setupWidgets();

  model = new QSqlTableModel(this);
}

TableWidget::TableWidget(QString table, QSqlDatabase *db, QWidget *parent)
  : AbstractTabWidget(parent)
{
  setupUi(this);
  setupWidgets();
  setTable(table, db);
}

QIcon TableWidget::icon()
{
  return IconManager::get("table");
}

QString TableWidget::id()
{
  return QString("t %1 on %2")
      .arg(m_table)
      .arg(m_db->databaseName());
}

void TableWidget::refresh()
{
  if(!m_db->isOpen())
    close();
}

void TableWidget::reload()
{
  if(!m_db->isOpen())
    return;

  if(!m_db->tables().contains(m_table))
  {
    qDebug("Unknown table !");
    return;
  }

  tableView->setTable(m_table, m_db);
}

void TableWidget::setTable( QString table, QSqlDatabase *db )
{
  this->m_table = table;
  this->m_db = db;

  reload();
}

void TableWidget::setupWidgets()
{
  connect(tableView, SIGNAL(reloadRequested()), this, SLOT(reload()));
}

QString TableWidget::table()
{
  return m_table;
}
<commit_msg>Bogue ouverture vues<commit_after>/*
 * This program is free software; you can redistribute it and/or modify it under
 * the terms of the GNU General Public License as published by the Free Software
 * Foundation; version 3 of the License.
 *
 * This program 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 General Public License for more
 * details.
 */

#include "../dbmanager.h"
#include "../iconmanager.h"
#include "tablewidget.h"

TableWidget::TableWidget(QWidget *parent)
  : AbstractTabWidget(parent)
{
  setupUi(this);

  setupWidgets();

  model = new QSqlTableModel(this);
}

TableWidget::TableWidget(QString table, QSqlDatabase *db, QWidget *parent)
  : AbstractTabWidget(parent)
{
  setupUi(this);
  setupWidgets();
  setTable(table, db);
}

QIcon TableWidget::icon()
{
  return IconManager::get("table");
}

QString TableWidget::id()
{
  return QString("t %1 on %2")
      .arg(m_table)
      .arg(m_db->databaseName());
}

void TableWidget::refresh()
{
  if(!m_db->isOpen())
    close();
}

void TableWidget::reload()
{
  if(!m_db->isOpen())
    return;

  if(!m_db->tables(QSql::Tables).contains(m_table)
    && !m_db->tables(QSql::Views).contains(m_table)
    && !m_db->tables(QSql::SystemTables).contains(m_table))
  {
    qDebug("Unknown table !");
    return;
  }

  tableView->setTable(m_table, m_db);
}

void TableWidget::setTable( QString table, QSqlDatabase *db )
{
  this->m_table = table;
  this->m_db = db;

  reload();
}

void TableWidget::setupWidgets()
{
  connect(tableView, SIGNAL(reloadRequested()), this, SLOT(reload()));
}

QString TableWidget::table()
{
  return m_table;
}
<|endoftext|>