<commit_before>#include <stdio.h>
#include <cmath>
#include <boost/shared_ptr.hpp>
#include <lcm/lcm-cpp.hpp>
#include "lcmtypes/drc_lcmtypes.hpp"
#include <ConciseArgs>
#include <string>
#include <cstring>
#include <fstream>
#include "CImg.h"
using namespace cimg_library;

using namespace std;
#include <path_util/path_util.h>

class App{
  public:
    App(boost::shared_ptr<lcm::LCM> &lcm_, bool is_left);

    ~App(){
    }

  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    bool is_left;
    string handtext;  // Put RIGHT / LEFT
    static const int TACTILE_THRESHOLD = 3000;
    static const int NTACTILE = 32;
    void sandiaTactileStateHandler(const lcm::ReceiveBuffer* rbuf,
                             const std::string& channel, const  drc::hand_tactile_state_t* msg);

    void readCfg(const std::string& path_cfg);
    void getHeatColor(const float v, unsigned char color [3]);
    CImg<unsigned char> orig_img;
    CImg<unsigned char> disp_img;
    CImgDisplay main_disp;
    int img_x[NTACTILE], img_y[NTACTILE];
};

App::App(boost::shared_ptr<lcm::LCM> &lcm_, bool is_left):
   lcm_(lcm_), is_left(is_left){
  cout << "enter APP()" << endl;

  lcm_->subscribe(is_left ? "SANDIA_LEFT_TACTILE_STATE":"SANDIA_RIGHT_TACTILE_STATE",
                                        &App::sandiaTactileStateHandler, this);

  std::string cfg_path = std::string(getConfigPath()) +"/subsystems/sandia_hands/";
  string path_img, path_cfg;
  string filename = "sandia_hand_r";
  path_img = cfg_path + filename + ".jpg";
  path_cfg = cfg_path + filename + ".cfg";

  readCfg(path_cfg);

  orig_img.load(path_img.c_str());

  disp_img = orig_img;
  main_disp.display(disp_img);
  main_disp.resize(orig_img.width()/2, orig_img.height()/2);
  cout << "leave APP()" << endl;
}

void App::readCfg(const std::string& path_cfg){
  //649 1194         # hand origin on img
  //8.17647058824    # 556/68 px over real metric
  int hand_orig_x, hand_orig_y;
  float pixel_over_real;
  string garbage;
  ifstream fin(path_cfg.c_str());
  if(!fin)
    cout << "open filepath " << path_cfg << " fail" << endl;
  fin >> hand_orig_x >> hand_orig_y;
  getline(fin, garbage);
  fin >> pixel_over_real;
  getline(fin, garbage);
  for(int i=0;i<NTACTILE;i++){
    int id; float rx, ry, rz;
    fin >> id >> rx >> ry >> rz;
    img_x[i] = rx*pixel_over_real + hand_orig_x;
    img_y[i] = -ry*pixel_over_real + hand_orig_y;
    cout << i << " " << img_x[i] << " " << img_y[i] << endl;
  }
  fin.close();
}

void App::getHeatColor(const float v, unsigned char color [3]){
  // v: [0~1)
  // color: RGB (0,0,0)~(255,255,255)
  if(v < 0.5){
    color[0] = (unsigned char)0;
    color[1] = (unsigned char)(2*256*v);
    color[2] = (unsigned char)(255-2*256*v);
  }
  else{
    color[0] = (unsigned char)(2*256*(v-0.5));
    color[1] = (unsigned char)(255-2*256*(v-0.5));
    color[2] = (unsigned char)(0);
  }
}

void App::sandiaTactileStateHandler(const lcm::ReceiveBuffer* rbuf,
     const std::string& channel, const  drc::hand_tactile_state_t* msg){

  // Draw hand img
  unsigned char color[3];

  disp_img = orig_img;
  for(size_t i=0;i<NTACTILE;i++){
    float v = (float)msg->palm[i] / 65536.0 * 2.5;
    if(v<0) v = 0;
    else if(v>=1) v=1-1e-8;
    getHeatColor(v, color);
    //cout << color[0] << " " << color[1] << " " << color[2] <<endl;
    //cout << i <<" " << img_x[i] << " " << img_y[i] <<endl;
    disp_img.draw_circle(img_x[i], img_y[i], /*int radius*/ 20, color);
  }
  main_disp.display(disp_img);
  //main_disp.wait();
  ///////////////////////

}

int main(int argc, char *argv[]){
  bool lhand=false, rhand=false;
  ConciseArgs opt(argc, (char**)argv);
  opt.add(rhand, "r", "right","Process right hand message");
  opt.add(lhand, "l", "left","Process left hand message");

  opt.parse();
  if(rhand && lhand){
    printf("Only one hand at a time.\n");
    return 1;
  }
  if(!rhand && !lhand){
    printf("Please specify a hand. Type -h for usage.\n");
    return 1;
  }

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM() );
  if(!lcm->good())
    return 1;

  App app(lcm, lhand);
  while(0 == lcm->handle());
  return 0;
}
<commit_msg>Inculde left hand config files in tactile viz<commit_after>#include <stdio.h>
#include <cmath>
#include <boost/shared_ptr.hpp>
#include <lcm/lcm-cpp.hpp>
#include "lcmtypes/drc_lcmtypes.hpp"
#include <ConciseArgs>
#include <string>
#include <cstring>
#include <fstream>
#include "CImg.h"
using namespace cimg_library;

using namespace std;
#include <path_util/path_util.h>

class App{
  public:
    App(boost::shared_ptr<lcm::LCM> &lcm_, bool is_left);

    ~App(){
    }

  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    bool is_left;
    string handtext;  // Put RIGHT / LEFT
    static const int TACTILE_THRESHOLD = 3000;
    static const int NTACTILE = 32;
    void sandiaTactileStateHandler(const lcm::ReceiveBuffer* rbuf,
                             const std::string& channel, const  drc::hand_tactile_state_t* msg);

    void readCfg(const std::string& path_cfg);
    void getHeatColor(const float v, unsigned char color [3]);
    CImg<unsigned char> orig_img;
    CImg<unsigned char> disp_img;
    CImgDisplay main_disp;
    int img_x[NTACTILE], img_y[NTACTILE];
};

App::App(boost::shared_ptr<lcm::LCM> &lcm_, bool is_left):
   lcm_(lcm_), is_left(is_left){
  cout << "enter APP()" << endl;

  lcm_->subscribe(is_left ? "SANDIA_LEFT_TACTILE_STATE":"SANDIA_RIGHT_TACTILE_STATE",
                                        &App::sandiaTactileStateHandler, this);

  std::string cfg_path = std::string(getConfigPath()) +"/subsystems/sandia_hands/";
  string path_img, path_cfg;
  string filename = is_left ? "sandia_hand_l" : "sandia_hand_r";
  path_img = cfg_path + filename + ".jpg";
  path_cfg = cfg_path + filename + ".cfg";

  readCfg(path_cfg);

  orig_img.load(path_img.c_str());

  disp_img = orig_img;
  main_disp.display(disp_img);
  main_disp.resize(orig_img.width()/2, orig_img.height()/2);
  cout << "leave APP()" << endl;
}

void App::readCfg(const std::string& path_cfg){
  int hand_orig_x, hand_orig_y;
  float pixel_over_real;
  string garbage;
  ifstream fin(path_cfg.c_str());
  if(!fin)
    cout << "open filepath " << path_cfg << " fail" << endl;
  fin >> hand_orig_x >> hand_orig_y;
  getline(fin, garbage);
  fin >> pixel_over_real;
  getline(fin, garbage);
  for(int i=0;i<NTACTILE;i++){
    int id; float rx, ry, rz;
    fin >> id >> rx >> ry >> rz;
    img_x[i] = rx*pixel_over_real + hand_orig_x;
    img_y[i] = -ry*pixel_over_real + hand_orig_y;
    cout << i << " " << img_x[i] << " " << img_y[i] << endl;
  }
  fin.close();
}

void App::getHeatColor(const float v, unsigned char color [3]){
  // v: [0~1)
  // color: RGB (0,0,0)~(255,255,255)
  if(v < 0.5){
    color[0] = (unsigned char)0;
    color[1] = (unsigned char)(2*256*v);
    color[2] = (unsigned char)(255-2*256*v);
  }
  else{
    color[0] = (unsigned char)(2*256*(v-0.5));
    color[1] = (unsigned char)(255-2*256*(v-0.5));
    color[2] = (unsigned char)(0);
  }
}

void App::sandiaTactileStateHandler(const lcm::ReceiveBuffer* rbuf,
     const std::string& channel, const  drc::hand_tactile_state_t* msg){

  // Draw hand img
  unsigned char color[3];

  disp_img = orig_img;
  for(size_t i=0;i<NTACTILE;i++){
    float v = (float)msg->palm[i] / 65536.0 * 2.5;
    if(v<0) v = 0;
    else if(v>=1) v=1-1e-8;
    getHeatColor(v, color);
    //cout << color[0] << " " << color[1] << " " << color[2] <<endl;
    //cout << i <<" " << img_x[i] << " " << img_y[i] <<endl;
    disp_img.draw_circle(img_x[i], img_y[i], /*int radius*/ 20, color);
  }
  main_disp.display(disp_img);
  //main_disp.wait();
  ///////////////////////

}

int main(int argc, char *argv[]){
  bool lhand=false, rhand=false;
  ConciseArgs opt(argc, (char**)argv);
  opt.add(rhand, "r", "right","Process right hand message");
  opt.add(lhand, "l", "left","Process left hand message");

  opt.parse();
  if(rhand && lhand){
    printf("Only one hand at a time.\n");
    return 1;
  }
  if(!rhand && !lhand){
    printf("Please specify a hand. Type -h for usage.\n");
    return 1;
  }

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM() );
  if(!lcm->good())
    return 1;

  App app(lcm, lhand);
  while(0 == lcm->handle());
  return 0;
}
<|endoftext|>


<commit_before>/* Copyright 2015 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#if defined(ENABLE_ONEDNN_OPENMP) && defined(ENABLE_MKL) && defined(_OPENMP)
#ifndef DNNL_AARCH64_USE_ACL
// Using LLVM's OpenMP header
#include "external/llvm_openmp/include/omp.h"
/* Added EIGEN_DONT_PARALLELIZE to avoid duplicating omp.h, please refer to
this link https://eigen.tuxfamily.org/dox/TopicMultiThreading.html for more
info. It does not have any negative impact on performance. */
#define EIGEN_DONT_PARALLELIZE
#else
#include "omp.h"  // NOLINT
#endif
#endif  // ENABLE_ONEDNN_OPENMP && ENABLE_MKL &&_OPENMP

#include "absl/base/call_once.h"
#include "absl/container/flat_hash_set.h"
#include "tensorflow/core/common_runtime/local_device.h"
#include "tensorflow/core/common_runtime/scoped_allocator.h"
#include "tensorflow/core/common_runtime/scoped_allocator_mgr.h"
#include "tensorflow/core/common_runtime/threadpool_device.h"
#include "tensorflow/core/framework/allocator.h"
#include "tensorflow/core/framework/allocator_registry.h"
#include "tensorflow/core/framework/device_base.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor.pb.h"
#include "tensorflow/core/framework/tensor_util.h"
#include "tensorflow/core/framework/types.h"
#include "tensorflow/core/graph/types.h"
#include "tensorflow/core/lib/hash/hash.h"
#include "tensorflow/core/platform/tracing.h"
#include "tensorflow/core/platform/types.h"
#include "tensorflow/core/public/session_options.h"
#include "tensorflow/core/util/util.h"

#ifdef INTEL_MKL
#include "tensorflow/core/common_runtime/mkl_cpu_allocator.h"
#include "tensorflow/core/platform/cpu_info.h"
#endif  // INTEL_MKL

namespace tensorflow {

ThreadPoolDevice::ThreadPoolDevice(const SessionOptions& options,
                                   const string& name, Bytes memory_limit,
                                   const DeviceLocality& locality,
                                   Allocator* allocator)
    : LocalDevice(options, Device::BuildDeviceAttributes(
                               name, DEVICE_CPU, memory_limit, locality)),
      allocator_(allocator),
      scoped_allocator_mgr_(new ScopedAllocatorMgr(name)) {
  auto s = NodeFileWriter::GetNodeFileWriterIfEnabled(name, env());
  if (!s.ok()) {
    LOG(ERROR) << s.status();
  } else {
    node_file_writer_ = *s;
    if (node_file_writer_) {
      LOG(INFO) << "Writing NodeDefs to file: "
                << node_file_writer_->filename();
    }
  }

#if defined(ENABLE_ONEDNN_OPENMP) && defined(INTEL_MKL)
  // Early return when MKL is disabled
  if (!IsMKLEnabled()) return;
#ifdef _OPENMP
  const char* user_omp_threads = getenv("OMP_NUM_THREADS");
  static absl::once_flag num_threads_setting_flag;
  if (user_omp_threads == nullptr) {
    // OMP_NUM_THREADS controls MKL's intra-op parallelization
    // Default to available physical cores
    const int mkl_intra_op = port::NumSchedulableCPUs();
    const int ht = port::NumHyperthreadsPerCore();
    absl::call_once(num_threads_setting_flag, omp_set_num_threads,
                    (mkl_intra_op + ht - 1) / ht);
  }

#ifndef DNNL_AARCH64_USE_ACL
  const char* user_kmp_blocktime = getenv("KMP_BLOCKTIME");
  static absl::once_flag blocktime_setting_flag;
  if (user_kmp_blocktime == nullptr) {
    // Sets the time, in milliseconds, that a thread should wait,
    // after completing the execution of a parallel region, before sleeping.
    absl::call_once(blocktime_setting_flag, kmp_set_blocktime, 1);
  }
#endif

#endif  // _OPENMP
#endif  // defined(ENABLE_ONEDNN_OPENMP) && defined(INTEL_MKL)
}

ThreadPoolDevice::~ThreadPoolDevice() {}

Allocator* ThreadPoolDevice::GetAllocator(AllocatorAttributes attr) {
  return allocator_;
}

Allocator* ThreadPoolDevice::GetScopedAllocator(AllocatorAttributes attr,
                                                int64_t step_id) {
  if (attr.scope_id > 0) {
    return scoped_allocator_mgr_->GetContainer(step_id)->GetInstance(
        attr.scope_id);
  }
  LOG(FATAL) << "Unexpected call to ThreadPoolDevice::GetScopedAllocator "
             << "attr.scope_id = " << attr.scope_id;
  return allocator_;
}

Status ThreadPoolDevice::MakeTensorFromProto(
    const TensorProto& tensor_proto, const AllocatorAttributes alloc_attrs,
    Tensor* tensor) {
  if (tensor_proto.dtype() > 0 && tensor_proto.dtype() <= DataType_MAX) {
    Tensor parsed(tensor_proto.dtype());
    if (parsed.FromProto(allocator_, tensor_proto)) {
      *tensor = std::move(parsed);
      return Status::OK();
    }
  }
  return errors::InvalidArgument("Cannot parse tensor from proto: ",
                                 tensor_proto.DebugString());
}

void ThreadPoolDevice::CopyTensorInSameDevice(
    const Tensor* input_tensor, Tensor* output_tensor,
    const DeviceContext* device_context, StatusCallback done) {
  if (input_tensor->NumElements() != output_tensor->NumElements()) {
    done(errors::Internal(
        "CPU->CPU copy shape mismatch: input=", input_tensor->shape(),
        ", output=", output_tensor->shape()));
    return;
  }
  tensor::DeepCopy(*input_tensor, output_tensor);
  done(Status::OK());
}

namespace {
const absl::flat_hash_set<std::string>* GetOpsToLogFromEnv() {
  auto* result = new absl::flat_hash_set<std::string>;
  const char* env = getenv("TF_CPU_DEBUG_OPS_TO_LOG");
  if (!env) {
    return result;
  }

  std::vector<absl::string_view> ops = absl::StrSplit(env, ',');
  LOG(INFO) << "Will log inputs & outputs from the following ops: ";
  for (absl::string_view op : ops) {
    result->insert(std::string(op));
    LOG(INFO) << "  |" << op << "|";
  }

  return result;
}

bool ShouldLogInputsAndOutputs(OpKernel* op_kernel) {
  static const absl::flat_hash_set<std::string>& ops_to_log =
      *GetOpsToLogFromEnv();
  return ops_to_log.count(op_kernel->type_string());
}
}  // namespace

void ThreadPoolDevice::Compute(OpKernel* op_kernel, OpKernelContext* context) {
  bool should_log_inputs_and_outputs = ShouldLogInputsAndOutputs(op_kernel);

  if (should_log_inputs_and_outputs) {
    LogInputs(op_kernel, context);
  }

  op_kernel->Compute(context);

  if (context->status().ok() && node_file_writer_) {
    Status s = node_file_writer_->RecordNodeExecution(op_kernel, context);
    if (!s.ok()) {
      LOG(ERROR) << s;
      context->SetStatus(s);
    }
  }

  if (should_log_inputs_and_outputs) {
    LogOutputs(op_kernel, context);
  }
}

void ThreadPoolDevice::ComputeAsync(AsyncOpKernel* op_kernel,
                                    OpKernelContext* context,
                                    AsyncOpKernel::DoneCallback done) {
  bool should_log_inputs_and_outputs = ShouldLogInputsAndOutputs(op_kernel);

  if (should_log_inputs_and_outputs) {
    LogInputs(op_kernel, context);
    AsyncOpKernel::DoneCallback parent_done = done;
    done = [this, parent_done, op_kernel, context]() {
      LogOutputs(op_kernel, context);
      parent_done();
    };
  }

  op_kernel->ComputeAsync(context, done);
}

void ThreadPoolDevice::LogInputs(OpKernel* op_kernel,
                                 OpKernelContext* context) {
  LOG(INFO) << "Inputs for " << op_kernel->name() << " (total "
            << context->num_inputs() << "):";
  for (int i = 0; i < context->num_inputs(); i++) {
    if (!context->has_input(i)) {
      LOG(INFO) << "input # " << i << " is absent";
      continue;
    }
    LOG(INFO) << "input # " << i;
    LOG(INFO) << context->input(i).DebugString(-1);
  }
  LOG(INFO) << "";
}

void ThreadPoolDevice::LogOutputs(OpKernel* op_kernel,
                                  OpKernelContext* context) {
  if (!context->status().ok()) {
    LOG(INFO) << op_kernel->name()
              << " failed: " << context->status().error_message();
    return;
  }

  LOG(INFO) << "Outputs for " << op_kernel->name() << " (total "
            << context->num_inputs() << "):";
  for (int i = 0; i < context->num_outputs(); i++) {
    Tensor* output = context->mutable_output(i);
    if (output == nullptr) {
      LOG(INFO) << "output # " << i << " is null";
    } else {
      LOG(INFO) << "output # " << i;
      LOG(INFO) << output->DebugString(-1);
    }
  }
  LOG(INFO) << "";
}

#ifdef INTEL_MKL
namespace {
class MklCPUAllocatorFactory : public AllocatorFactory {
 public:
  bool NumaEnabled() override { return false; }

  Allocator* CreateAllocator() override { return new MklCPUAllocator; }

  // Note: Ignores numa_node, for now.
  virtual SubAllocator* CreateSubAllocator(int numa_node) {
    return new MklSubAllocator;
  }
};

REGISTER_MEM_ALLOCATOR("MklCPUAllocator", (IsMKLEnabled() ? 200 : 50),
                       MklCPUAllocatorFactory);

}  // namespace
#endif  // INTEL_MKL

}  // namespace tensorflow
<commit_msg>Avoid expensive string building in the case where op input and output logging is completely disabled.<commit_after>/* Copyright 2015 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#if defined(ENABLE_ONEDNN_OPENMP) && defined(ENABLE_MKL) && defined(_OPENMP)
#ifndef DNNL_AARCH64_USE_ACL
// Using LLVM's OpenMP header
#include "external/llvm_openmp/include/omp.h"
/* Added EIGEN_DONT_PARALLELIZE to avoid duplicating omp.h, please refer to
this link https://eigen.tuxfamily.org/dox/TopicMultiThreading.html for more
info. It does not have any negative impact on performance. */
#define EIGEN_DONT_PARALLELIZE
#else
#include "omp.h"  // NOLINT
#endif
#endif  // ENABLE_ONEDNN_OPENMP && ENABLE_MKL &&_OPENMP

#include "absl/base/call_once.h"
#include "absl/container/flat_hash_set.h"
#include "tensorflow/core/common_runtime/local_device.h"
#include "tensorflow/core/common_runtime/scoped_allocator.h"
#include "tensorflow/core/common_runtime/scoped_allocator_mgr.h"
#include "tensorflow/core/common_runtime/threadpool_device.h"
#include "tensorflow/core/framework/allocator.h"
#include "tensorflow/core/framework/allocator_registry.h"
#include "tensorflow/core/framework/device_base.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor.pb.h"
#include "tensorflow/core/framework/tensor_util.h"
#include "tensorflow/core/framework/types.h"
#include "tensorflow/core/graph/types.h"
#include "tensorflow/core/lib/hash/hash.h"
#include "tensorflow/core/platform/tracing.h"
#include "tensorflow/core/platform/types.h"
#include "tensorflow/core/public/session_options.h"
#include "tensorflow/core/util/util.h"

#ifdef INTEL_MKL
#include "tensorflow/core/common_runtime/mkl_cpu_allocator.h"
#include "tensorflow/core/platform/cpu_info.h"
#endif  // INTEL_MKL

namespace tensorflow {

ThreadPoolDevice::ThreadPoolDevice(const SessionOptions& options,
                                   const string& name, Bytes memory_limit,
                                   const DeviceLocality& locality,
                                   Allocator* allocator)
    : LocalDevice(options, Device::BuildDeviceAttributes(
                               name, DEVICE_CPU, memory_limit, locality)),
      allocator_(allocator),
      scoped_allocator_mgr_(new ScopedAllocatorMgr(name)) {
  auto s = NodeFileWriter::GetNodeFileWriterIfEnabled(name, env());
  if (!s.ok()) {
    LOG(ERROR) << s.status();
  } else {
    node_file_writer_ = *s;
    if (node_file_writer_) {
      LOG(INFO) << "Writing NodeDefs to file: "
                << node_file_writer_->filename();
    }
  }

#if defined(ENABLE_ONEDNN_OPENMP) && defined(INTEL_MKL)
  // Early return when MKL is disabled
  if (!IsMKLEnabled()) return;
#ifdef _OPENMP
  const char* user_omp_threads = getenv("OMP_NUM_THREADS");
  static absl::once_flag num_threads_setting_flag;
  if (user_omp_threads == nullptr) {
    // OMP_NUM_THREADS controls MKL's intra-op parallelization
    // Default to available physical cores
    const int mkl_intra_op = port::NumSchedulableCPUs();
    const int ht = port::NumHyperthreadsPerCore();
    absl::call_once(num_threads_setting_flag, omp_set_num_threads,
                    (mkl_intra_op + ht - 1) / ht);
  }

#ifndef DNNL_AARCH64_USE_ACL
  const char* user_kmp_blocktime = getenv("KMP_BLOCKTIME");
  static absl::once_flag blocktime_setting_flag;
  if (user_kmp_blocktime == nullptr) {
    // Sets the time, in milliseconds, that a thread should wait,
    // after completing the execution of a parallel region, before sleeping.
    absl::call_once(blocktime_setting_flag, kmp_set_blocktime, 1);
  }
#endif

#endif  // _OPENMP
#endif  // defined(ENABLE_ONEDNN_OPENMP) && defined(INTEL_MKL)
}

ThreadPoolDevice::~ThreadPoolDevice() {}

Allocator* ThreadPoolDevice::GetAllocator(AllocatorAttributes attr) {
  return allocator_;
}

Allocator* ThreadPoolDevice::GetScopedAllocator(AllocatorAttributes attr,
                                                int64_t step_id) {
  if (attr.scope_id > 0) {
    return scoped_allocator_mgr_->GetContainer(step_id)->GetInstance(
        attr.scope_id);
  }
  LOG(FATAL) << "Unexpected call to ThreadPoolDevice::GetScopedAllocator "
             << "attr.scope_id = " << attr.scope_id;
  return allocator_;
}

Status ThreadPoolDevice::MakeTensorFromProto(
    const TensorProto& tensor_proto, const AllocatorAttributes alloc_attrs,
    Tensor* tensor) {
  if (tensor_proto.dtype() > 0 && tensor_proto.dtype() <= DataType_MAX) {
    Tensor parsed(tensor_proto.dtype());
    if (parsed.FromProto(allocator_, tensor_proto)) {
      *tensor = std::move(parsed);
      return Status::OK();
    }
  }
  return errors::InvalidArgument("Cannot parse tensor from proto: ",
                                 tensor_proto.DebugString());
}

void ThreadPoolDevice::CopyTensorInSameDevice(
    const Tensor* input_tensor, Tensor* output_tensor,
    const DeviceContext* device_context, StatusCallback done) {
  if (input_tensor->NumElements() != output_tensor->NumElements()) {
    done(errors::Internal(
        "CPU->CPU copy shape mismatch: input=", input_tensor->shape(),
        ", output=", output_tensor->shape()));
    return;
  }
  tensor::DeepCopy(*input_tensor, output_tensor);
  done(Status::OK());
}

namespace {
const absl::flat_hash_set<std::string>* GetOpsToLogFromEnv() {
  auto* result = new absl::flat_hash_set<std::string>;
  const char* env = getenv("TF_CPU_DEBUG_OPS_TO_LOG");
  if (!env) {
    return result;
  }

  std::vector<absl::string_view> ops = absl::StrSplit(env, ',');
  LOG(INFO) << "Will log inputs & outputs from the following ops: ";
  for (absl::string_view op : ops) {
    result->insert(std::string(op));
    LOG(INFO) << "  |" << op << "|";
  }

  return result;
}

bool ShouldLogInputsAndOutputs(OpKernel* op_kernel) {
  static const absl::flat_hash_set<std::string>& ops_to_log =
      *GetOpsToLogFromEnv();
  static const bool is_empty = ops_to_log.empty();
  if (is_empty) {
    return false;
  }
  return ops_to_log.count(op_kernel->type_string());
}
}  // namespace

void ThreadPoolDevice::Compute(OpKernel* op_kernel, OpKernelContext* context) {
  bool should_log_inputs_and_outputs = ShouldLogInputsAndOutputs(op_kernel);

  if (should_log_inputs_and_outputs) {
    LogInputs(op_kernel, context);
  }

  op_kernel->Compute(context);

  if (context->status().ok() && node_file_writer_) {
    Status s = node_file_writer_->RecordNodeExecution(op_kernel, context);
    if (!s.ok()) {
      LOG(ERROR) << s;
      context->SetStatus(s);
    }
  }

  if (should_log_inputs_and_outputs) {
    LogOutputs(op_kernel, context);
  }
}

void ThreadPoolDevice::ComputeAsync(AsyncOpKernel* op_kernel,
                                    OpKernelContext* context,
                                    AsyncOpKernel::DoneCallback done) {
  bool should_log_inputs_and_outputs = ShouldLogInputsAndOutputs(op_kernel);

  if (should_log_inputs_and_outputs) {
    LogInputs(op_kernel, context);
    AsyncOpKernel::DoneCallback parent_done = done;
    done = [this, parent_done, op_kernel, context]() {
      LogOutputs(op_kernel, context);
      parent_done();
    };
  }

  op_kernel->ComputeAsync(context, done);
}

void ThreadPoolDevice::LogInputs(OpKernel* op_kernel,
                                 OpKernelContext* context) {
  LOG(INFO) << "Inputs for " << op_kernel->name() << " (total "
            << context->num_inputs() << "):";
  for (int i = 0; i < context->num_inputs(); i++) {
    if (!context->has_input(i)) {
      LOG(INFO) << "input # " << i << " is absent";
      continue;
    }
    LOG(INFO) << "input # " << i;
    LOG(INFO) << context->input(i).DebugString(-1);
  }
  LOG(INFO) << "";
}

void ThreadPoolDevice::LogOutputs(OpKernel* op_kernel,
                                  OpKernelContext* context) {
  if (!context->status().ok()) {
    LOG(INFO) << op_kernel->name()
              << " failed: " << context->status().error_message();
    return;
  }

  LOG(INFO) << "Outputs for " << op_kernel->name() << " (total "
            << context->num_inputs() << "):";
  for (int i = 0; i < context->num_outputs(); i++) {
    Tensor* output = context->mutable_output(i);
    if (output == nullptr) {
      LOG(INFO) << "output # " << i << " is null";
    } else {
      LOG(INFO) << "output # " << i;
      LOG(INFO) << output->DebugString(-1);
    }
  }
  LOG(INFO) << "";
}

#ifdef INTEL_MKL
namespace {
class MklCPUAllocatorFactory : public AllocatorFactory {
 public:
  bool NumaEnabled() override { return false; }

  Allocator* CreateAllocator() override { return new MklCPUAllocator; }

  // Note: Ignores numa_node, for now.
  virtual SubAllocator* CreateSubAllocator(int numa_node) {
    return new MklSubAllocator;
  }
};

REGISTER_MEM_ALLOCATOR("MklCPUAllocator", (IsMKLEnabled() ? 200 : 50),
                       MklCPUAllocatorFactory);

}  // namespace
#endif  // INTEL_MKL

}  // namespace tensorflow
<|endoftext|>


<commit_before>/* Copyright 2016 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/core/platform/profile_utils/cpu_utils.h"

#include <fstream>
#include <limits>
#include <mutex>

#if defined(_WIN32)
#include <windows.h>
#endif

#if defined(__APPLE__)
#include <sys/sysctl.h>
#endif

#include "absl/base/call_once.h"
#include "tensorflow/core/platform/logging.h"
#include "tensorflow/core/platform/profile_utils/android_armv7a_cpu_utils_helper.h"

namespace tensorflow {
namespace profile_utils {

/* static */ constexpr int64 CpuUtils::INVALID_FREQUENCY;

static ICpuUtilsHelper* cpu_utils_helper_instance_ = nullptr;

#if (defined(__powerpc__) ||                                             \
     defined(__ppc__) && (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__)) || \
    (defined(__s390x__))
/* static */ uint64 CpuUtils::GetCycleCounterFrequency() {
  static const uint64 cpu_frequency = GetCycleCounterFrequencyImpl();
  return cpu_frequency;
}
#else
/* static */ int64 CpuUtils::GetCycleCounterFrequency() {
  static const int64 cpu_frequency = GetCycleCounterFrequencyImpl();
  return cpu_frequency;
}
#endif

/* static */ double CpuUtils::GetMicroSecPerClock() {
  static const double micro_sec_per_clock =
      (1000.0 * 1000.0) / static_cast<double>(GetCycleCounterFrequency());
  return micro_sec_per_clock;
}

/* static */ void CpuUtils::ResetClockCycle() {
  GetCpuUtilsHelperSingletonInstance().ResetClockCycle();
}

/* static */ void CpuUtils::EnableClockCycleProfiling() {
  GetCpuUtilsHelperSingletonInstance().EnableClockCycleProfiling();
}

/* static */ void CpuUtils::DisableClockCycleProfiling() {
  GetCpuUtilsHelperSingletonInstance().DisableClockCycleProfiling();
}

/* static */ std::chrono::duration<double> CpuUtils::ConvertClockCycleToTime(
    const int64 clock_cycle) {
  return std::chrono::duration<double>(static_cast<double>(clock_cycle) /
                                       GetCycleCounterFrequency());
}

/* static */ int64 CpuUtils::GetCycleCounterFrequencyImpl() {
// TODO(satok): do not switch by macro here
#if defined(__ANDROID__)
  return GetCpuUtilsHelperSingletonInstance().CalculateCpuFrequency();
#elif defined(__linux__)
  // Read the contents of /proc/cpuinfo.
  std::ifstream cpuinfo("/proc/cpuinfo");
  if (!cpuinfo) {
    LOG(WARNING) << "Failed to open /proc/cpuinfo";
    return INVALID_FREQUENCY;
  }
  string line;
  while (std::getline(cpuinfo, line)) {
    double cpu_freq = 0.0;
    int retval = 0;
    double freq_factor = 2.0;
#if (defined(__powerpc__) || \
     defined(__ppc__) && (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__))
    retval = sscanf(line.c_str(), "clock              : %lfMHz", &cpu_freq);
    freq_factor = 1.0;
#elif defined(__s390x__)
    retval = sscanf(line.c_str(), "bogomips per cpu: %lf", &cpu_freq);
#else
    retval = sscanf(line.c_str(), "bogomips : %lf", &cpu_freq);
#endif
    if (retval > 0) {
      const double freq_ghz = cpu_freq / 1000.0 / freq_factor;
      if (retval != 1 || freq_ghz < 0.01) {
        LOG(WARNING) << "Failed to get CPU frequency: " << freq_ghz << " GHz";
        return INVALID_FREQUENCY;
      }
      const int64 freq_n =
          static_cast<int64>(freq_ghz * 1000.0 * 1000.0 * 1000.0);
      LOG(INFO) << "CPU Frequency: " << freq_n << " Hz";
      return freq_n;
    }
  }
  LOG(WARNING)
      << "Failed to find bogomips or clock in /proc/cpuinfo; cannot determine "
         "CPU frequency";
  return INVALID_FREQUENCY;
#elif defined(__APPLE__)
  int64 freq_hz = 0;
  size_t freq_size = sizeof(freq_hz);
  int retval =
      sysctlbyname("hw.cpufrequency_max", &freq_hz, &freq_size, NULL, 0);
  if (retval != 0 || freq_hz < 1e6) {
    LOG(WARNING) << "Failed to get CPU frequency: " << freq_hz << " Hz";
    return INVALID_FREQUENCY;
  }
  return freq_hz;
#elif defined(_WIN32)
  LARGE_INTEGER freq;
  QueryPerformanceFrequency(&freq);
  return freq.QuadPart;
#else
  // TODO(satok): Support other OS if needed
  // Return INVALID_FREQUENCY on unsupported OS
  return INVALID_FREQUENCY;
#endif
}

/* static */ ICpuUtilsHelper& CpuUtils::GetCpuUtilsHelperSingletonInstance() {
  static absl::once_flag flag;
  absl::call_once(flag, []() {
    if (cpu_utils_helper_instance_ != nullptr) {
      LOG(FATAL) << "cpu_utils_helper_instance_ is already instantiated.";
    }
#if defined(__ANDROID__) && (__ANDROID_API__ >= 21) && \
    (defined(__ARM_ARCH_7A__) || defined(__aarch64__))
    cpu_utils_helper_instance_ = new AndroidArmV7ACpuUtilsHelper();
#else
      cpu_utils_helper_instance_ = new DefaultCpuUtilsHelper();
#endif
  });
  return *cpu_utils_helper_instance_;
}

}  // namespace profile_utils
}  // namespace tensorflow
<commit_msg>find bogomips on aarch64<commit_after>/* Copyright 2016 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/core/platform/profile_utils/cpu_utils.h"

#include <fstream>
#include <limits>
#include <mutex>

#if defined(_WIN32)
#include <windows.h>
#endif

#if defined(__APPLE__)
#include <sys/sysctl.h>
#endif

#include "absl/base/call_once.h"
#include "tensorflow/core/platform/logging.h"
#include "tensorflow/core/platform/profile_utils/android_armv7a_cpu_utils_helper.h"

namespace tensorflow {
namespace profile_utils {

/* static */ constexpr int64 CpuUtils::INVALID_FREQUENCY;

static ICpuUtilsHelper* cpu_utils_helper_instance_ = nullptr;

#if (defined(__powerpc__) ||                                             \
     defined(__ppc__) && (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__)) || \
    (defined(__s390x__))
/* static */ uint64 CpuUtils::GetCycleCounterFrequency() {
  static const uint64 cpu_frequency = GetCycleCounterFrequencyImpl();
  return cpu_frequency;
}
#else
/* static */ int64 CpuUtils::GetCycleCounterFrequency() {
  static const int64 cpu_frequency = GetCycleCounterFrequencyImpl();
  return cpu_frequency;
}
#endif

/* static */ double CpuUtils::GetMicroSecPerClock() {
  static const double micro_sec_per_clock =
      (1000.0 * 1000.0) / static_cast<double>(GetCycleCounterFrequency());
  return micro_sec_per_clock;
}

/* static */ void CpuUtils::ResetClockCycle() {
  GetCpuUtilsHelperSingletonInstance().ResetClockCycle();
}

/* static */ void CpuUtils::EnableClockCycleProfiling() {
  GetCpuUtilsHelperSingletonInstance().EnableClockCycleProfiling();
}

/* static */ void CpuUtils::DisableClockCycleProfiling() {
  GetCpuUtilsHelperSingletonInstance().DisableClockCycleProfiling();
}

/* static */ std::chrono::duration<double> CpuUtils::ConvertClockCycleToTime(
    const int64 clock_cycle) {
  return std::chrono::duration<double>(static_cast<double>(clock_cycle) /
                                       GetCycleCounterFrequency());
}

/* static */ int64 CpuUtils::GetCycleCounterFrequencyImpl() {
// TODO(satok): do not switch by macro here
#if defined(__ANDROID__)
  return GetCpuUtilsHelperSingletonInstance().CalculateCpuFrequency();
#elif defined(__linux__)
  // Read the contents of /proc/cpuinfo.
  std::ifstream cpuinfo("/proc/cpuinfo");
  if (!cpuinfo) {
    LOG(WARNING) << "Failed to open /proc/cpuinfo";
    return INVALID_FREQUENCY;
  }
  string line;
  while (std::getline(cpuinfo, line)) {
    double cpu_freq = 0.0;
    int retval = 0;
    double freq_factor = 2.0;
#if (defined(__powerpc__) || \
     defined(__ppc__) && (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__))
    retval = sscanf(line.c_str(), "clock              : %lfMHz", &cpu_freq);
    freq_factor = 1.0;
#elif defined(__s390x__)
    retval = sscanf(line.c_str(), "bogomips per cpu: %lf", &cpu_freq);
#elif defined(__aarch64__)
    retval = sscanf(line.c_str(), "BogoMIPS : %lf", &cpu_freq);
#else
    retval = sscanf(line.c_str(), "bogomips : %lf", &cpu_freq);
#endif
    if (retval > 0) {
      const double freq_ghz = cpu_freq / 1000.0 / freq_factor;
      if (retval != 1 || freq_ghz < 0.01) {
        LOG(WARNING) << "Failed to get CPU frequency: " << freq_ghz << " GHz";
        return INVALID_FREQUENCY;
      }
      const int64 freq_n =
          static_cast<int64>(freq_ghz * 1000.0 * 1000.0 * 1000.0);
      LOG(INFO) << "CPU Frequency: " << freq_n << " Hz";
      return freq_n;
    }
  }
  LOG(WARNING)
      << "Failed to find bogomips or clock in /proc/cpuinfo; cannot determine "
         "CPU frequency";
  return INVALID_FREQUENCY;
#elif defined(__APPLE__)
  int64 freq_hz = 0;
  size_t freq_size = sizeof(freq_hz);
  int retval =
      sysctlbyname("hw.cpufrequency_max", &freq_hz, &freq_size, NULL, 0);
  if (retval != 0 || freq_hz < 1e6) {
    LOG(WARNING) << "Failed to get CPU frequency: " << freq_hz << " Hz";
    return INVALID_FREQUENCY;
  }
  return freq_hz;
#elif defined(_WIN32)
  LARGE_INTEGER freq;
  QueryPerformanceFrequency(&freq);
  return freq.QuadPart;
#else
  // TODO(satok): Support other OS if needed
  // Return INVALID_FREQUENCY on unsupported OS
  return INVALID_FREQUENCY;
#endif
}

/* static */ ICpuUtilsHelper& CpuUtils::GetCpuUtilsHelperSingletonInstance() {
  static absl::once_flag flag;
  absl::call_once(flag, []() {
    if (cpu_utils_helper_instance_ != nullptr) {
      LOG(FATAL) << "cpu_utils_helper_instance_ is already instantiated.";
    }
#if defined(__ANDROID__) && (__ANDROID_API__ >= 21) && \
    (defined(__ARM_ARCH_7A__) || defined(__aarch64__))
    cpu_utils_helper_instance_ = new AndroidArmV7ACpuUtilsHelper();
#else
      cpu_utils_helper_instance_ = new DefaultCpuUtilsHelper();
#endif
  });
  return *cpu_utils_helper_instance_;
}

}  // namespace profile_utils
}  // namespace tensorflow
<|endoftext|>


<commit_before>/* Copyright 2019 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/lite/delegates/gpu/cl/kernels/pooling.h"

#include <string>

#include "tensorflow/lite/delegates/gpu/cl/kernels/util.h"
#include "tensorflow/lite/delegates/gpu/cl/kernels/work_group_picking.h"

namespace tflite {
namespace gpu {
namespace cl {
namespace {

std::string GetAveragePoolingKernelCode(
    const TensorDescriptor& src_descriptor,
    const TensorDescriptor& dst_descriptor, CalculationsPrecision precision,
    const CLDevice& device,
    const std::vector<ElementwiseOperation*>& linked_operations) {
  TensorCodeGenerator src_tensor("src_data", "src_size", src_descriptor);
  TensorCodeGenerator dst_tensor("dst_data", "dst_size", dst_descriptor);

  const auto address_mode = GetFastestZeroMode(device);

  std::string code = GetCommonDefines(precision);

  const bool manual_clamp =
      src_descriptor.storage_type == TensorStorageType::BUFFER ||
      src_descriptor.storage_type == TensorStorageType::IMAGE_BUFFER;

  code += "__kernel void main_function(\n";
  code += src_tensor.GetDeclaration(AccessType::READ);
  code += GetArgsDeclaration(linked_operations);
  code += dst_tensor.GetDeclaration(AccessType::WRITE) + ",\n";
  code += "    int4 src_size,             \n";
  code += "    int4 dst_size,             \n";
  code += "    int2 kernel_size,          \n";
  code += "    int2 padding,              \n";
  code += "    int2 stride                \n";
  code += ") {\n";
  code += "  int X = get_global_id(0);\n";
  code += "  int Y = get_global_id(1);\n";
  code += "  int Z = get_global_id(2);\n";
  code +=
      "  if (X >= dst_size.x || Y >= dst_size.y || Z >= dst_size.w) return; \n";
  code += "  float4 r = (float4)(0.0f);\n";
  code += "  float window_size = 0.0;\n";
  code += "  for (int ky = 0; ky < kernel_size.y; ++ky) {\n";
  code += "    int y_c = Y * stride.y - padding.y + ky;\n";
  code += "    bool outside_y = y_c < 0 || y_c >= src_size.y;\n";
  code += "    for (int kx = 0; kx < kernel_size.x; ++kx) {\n";
  code += "      int x_c = X * stride.x - padding.x + kx;\n";
  code += "      bool outside = outside_y || x_c < 0 || x_c >= src_size.x;\n";
  if (manual_clamp) {
    code += "     r += !outside ? " +
            src_tensor.ReadAsFloat3D("x_c", "y_c", "Z",
                                     TextureAddressMode::DONT_CARE) +
            " : (float4)(0.0f);\n";
  } else {
    code += "      r += " +
            src_tensor.ReadAsFloat3D("x_c", "y_c", "Z", address_mode) + ";\n";
  }
  code += "        window_size += !outside ? 1.0 : 0.0;\n";
  code += "    }\n";
  code += "  }\n";
  // If window_size==0, window covered nothing. This situation is a sign of
  // incorrectly constructed operation. NaNs are expected as output.
  code += "  FLT4 result = TO_FLT4(r / window_size);\n";
  const LinkingContext context{"result", "X", "Y", "Z"};
  code += PostProcess(linked_operations, context);
  code += "  " + dst_tensor.Write3D("result", "X", "Y", "Z");
  code += "}\n";

  return code;
}

std::string GetMaxPoolingKernelCode(
    const TensorDescriptor& src_descriptor,
    const TensorDescriptor& dst_descriptor, CalculationsPrecision precision,
    const std::vector<ElementwiseOperation*>& linked_operations,
    bool output_indices) {
  TensorCodeGenerator src_tensor("src_data", "src_size", src_descriptor);
  TensorCodeGenerator dst_tensor("dst_data", "dst_size", dst_descriptor);
  TensorCodeGenerator indices_tensor("dst_indices", "dst_size", dst_descriptor);

  std::string code = GetCommonDefines(precision);

  code += "__kernel void main_function(\n";
  code += src_tensor.GetDeclaration(AccessType::READ);
  code += GetArgsDeclaration(linked_operations);
  code += dst_tensor.GetDeclaration(AccessType::WRITE) + ",\n";
  if (output_indices) {
    code += indices_tensor.GetDeclaration(AccessType::WRITE) + ",\n";
  }
  code += "    int4 src_size,             \n";
  code += "    int4 dst_size,             \n";
  code += "    int2 kernel_size,          \n";
  code += "    int2 padding,              \n";
  code += "    int2 stride                \n";
  code += ") {\n";
  code += "  int X = get_global_id(0);\n";
  code += "  int Y = get_global_id(1);\n";
  code += "  int Z = get_global_id(2);\n";
  code +=
      "  if (X >= dst_size.x || Y >= dst_size.y || Z >= dst_size.w) return; \n";
  code += "  FLT4 maximum = (FLT4)(-10000.0f);\n";
  if (output_indices) {
    code += "  FLT4 indexes = (FLT4)(0.0f);\n";
    code += "  FLT index_counter = (FLT)(0.1f);\n";
  }
  code += "  for (int ky = 0; ky < kernel_size.y; ++ky) {\n";
  code += "    int y_c = Y * stride.y - padding.y + ky;\n";
  code += "    bool outside_y = y_c < 0 || y_c >= src_size.y;\n";
  code += "    for (int kx = 0; kx < kernel_size.x; ++kx) {\n";
  code += "      int x_c = X * stride.x - padding.x + kx;\n";
  code += "      bool outside_x = x_c < 0 || x_c >= src_size.x;\n";
  code += "      if (!outside_x && !outside_y) {\n";
  code += "        FLT4 src = " +
          src_tensor.Read3D("x_c", "y_c", "Z", TextureAddressMode::DONT_CARE) +
          ";\n";
  if (output_indices) {
    code += "        if (src.x > maximum.x) {\n";
    code += "          indexes.x = index_counter;\n";
    code += "          maximum.x = src.x;\n";
    code += "        }\n";
    code += "        if (src.y > maximum.y) {\n";
    code += "          indexes.y = index_counter;\n";
    code += "          maximum.y = src.y;\n";
    code += "        }\n";
    code += "        if (src.z > maximum.z) {\n";
    code += "          indexes.z = index_counter;\n";
    code += "          maximum.z = src.z;\n";
    code += "        }\n";
    code += "        if (src.w > maximum.w) {\n";
    code += "          indexes.w = index_counter;\n";
    code += "          maximum.w = src.w;\n";
    code += "        }\n";
    code += "        index_counter += (FLT)(1.0f);\n";
  }
  code += "        maximum = max(src, maximum);\n";
  code += "      };\n";
  code += "    }\n";
  code += "  }\n";
  const LinkingContext context{"maximum", "X", "Y", "Z"};
  code += PostProcess(linked_operations, context);
  code += "  " + dst_tensor.Write3D("maximum", "X", "Y", "Z");
  if (output_indices) {
    code += "  " + indices_tensor.Write3D("indexes", "X", "Y", "Z");
  }
  code += "}\n";

  return code;
}

}  // namespace

Pooling::Pooling(const OperationDef& definition,
                 const Pooling2DAttributes& attr)
    : GPUOperation(definition),
      stride_(attr.strides.w, attr.strides.h),
      padding_(attr.padding.prepended.w, attr.padding.prepended.h),
      kernel_size_(attr.kernel.w, attr.kernel.h),
      type_(attr.type),
      output_indices_(attr.output_indices) {}

Pooling::Pooling(Pooling&& kernel)
    : GPUOperation(std::move(kernel)),
      stride_(kernel.stride_),
      padding_(kernel.padding_),
      kernel_size_(kernel.kernel_size_),
      type_(kernel.type_),
      output_indices_(kernel.output_indices_),
      kernel_(std::move(kernel.kernel_)),
      work_group_size_(kernel.work_group_size_) {}

Pooling& Pooling::operator=(Pooling&& kernel) {
  if (this != &kernel) {
    std::swap(stride_, kernel.stride_);
    std::swap(padding_, kernel.padding_);
    std::swap(kernel_size_, kernel.kernel_size_);
    std::swap(type_, kernel.type_);
    std::swap(output_indices_, kernel.output_indices_);
    kernel_ = std::move(kernel.kernel_);
    std::swap(work_group_size_, kernel.work_group_size_);
    GPUOperation::operator=(std::move(kernel));
  }
  return *this;
}

Status Pooling::Compile(const CreationContext& creation_context) {
  std::string code;
  switch (type_) {
    case PoolingType::AVERAGE:
      code = GetAveragePoolingKernelCode(
          definition_.src_tensors[0], definition_.dst_tensors[0],
          definition_.precision, *creation_context.device, linked_operations_);
      break;
    case PoolingType::MAX:
      code = GetMaxPoolingKernelCode(
          definition_.src_tensors[0], definition_.dst_tensors[0],
          definition_.precision, linked_operations_, output_indices_);
      break;
    default:
      return InvalidArgumentError(
          "You should create another kernel with this params");
      break;
  }
  return creation_context.cache->GetOrCreateCLKernel(
      code, "main_function", *creation_context.context,
      *creation_context.device, &kernel_);
}

Status Pooling::BindArguments() {
  kernel_.ResetBindingCounter();
  RETURN_IF_ERROR(kernel_.SetMemoryAuto(src_[0]->GetMemoryPtr()));
  RETURN_IF_ERROR(BindArgs(&kernel_, linked_operations_));
  RETURN_IF_ERROR(kernel_.SetMemoryAuto(dst_[0]->GetMemoryPtrForWriting()));
  if (output_indices_) {
    RETURN_IF_ERROR(kernel_.SetMemoryAuto(dst_[1]->GetMemoryPtrForWriting()));
  }
  RETURN_IF_ERROR(kernel_.SetBytesAuto(src_[0]->GetSizeWithDepth()));
  RETURN_IF_ERROR(kernel_.SetBytesAuto(dst_[0]->GetSizeWithDepth()));
  RETURN_IF_ERROR(kernel_.SetBytesAuto(kernel_size_));
  RETURN_IF_ERROR(kernel_.SetBytesAuto(padding_));
  RETURN_IF_ERROR(kernel_.SetBytesAuto(stride_));

  return OkStatus();
}

int3 Pooling::GetGridSize() const {
  const int grid_x = dst_[0]->Width();
  const int grid_y = dst_[0]->Height();
  const int grid_z = dst_[0]->Depth();
  return int3(grid_x, grid_y, grid_z);
}

Status Pooling::Tune(const TuningParameters& params) {
  RETURN_IF_ERROR(BindArguments());
  return GetBestWorkGroup(params, kernel_, GetGridSize(), &work_group_size_);
}

Status Pooling::AddToQueue(CLCommandQueue* queue) {
  RETURN_IF_ERROR(BindArguments());
  return queue->DispatchImplicit(kernel_, GetGridSize(), work_group_size_);
}

Pooling CreatePooling(const OperationDef& definition,
                      const Pooling2DAttributes& attr) {
  return Pooling(definition, attr);
}

}  // namespace cl
}  // namespace gpu
}  // namespace tflite
<commit_msg>Batch support for Pooling.<commit_after>/* Copyright 2019 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/lite/delegates/gpu/cl/kernels/pooling.h"

#include <string>

#include "tensorflow/lite/delegates/gpu/cl/kernels/util.h"
#include "tensorflow/lite/delegates/gpu/cl/kernels/work_group_picking.h"

namespace tflite {
namespace gpu {
namespace cl {
namespace {

std::string GetAveragePoolingKernelCode(
    const OperationDef& op_def, bool stride_correction, const CLDevice& device,
    const std::vector<ElementwiseOperation*>& linked_operations) {
  TensorCodeGenerator src_tensor("src_data",
                                 {"src_size.x", "src_size.y", "src_size.z"},
                                 op_def.src_tensors[0]);
  TensorCodeGenerator dst_tensor("dst_data",
                                 {"dst_size.x", "dst_size.y", "dst_size.z"},
                                 op_def.dst_tensors[0]);

  const auto address_mode = GetFastestZeroMode(device);

  std::string c = GetCommonDefines(op_def.precision);

  const bool manual_clamp =
      op_def.src_tensors[0].storage_type == TensorStorageType::BUFFER ||
      op_def.src_tensors[0].storage_type == TensorStorageType::IMAGE_BUFFER;

  c += "__kernel void main_function(\n";
  c += src_tensor.GetDeclaration(AccessType::READ);
  c += GetArgsDeclaration(linked_operations);
  c += dst_tensor.GetDeclaration(AccessType::WRITE) + ",\n";
  c += "    int4 src_size,             \n";
  c += "    int4 dst_size,             \n";
  c += "    int2 kernel_size,          \n";
  c += "    int2 padding,              \n";
  c += "    int2 stride                \n";
  c += ") {\n";
  c += "  int X = get_global_id(0);\n";
  c += "  int Y = get_global_id(1);\n";
  c += "  int Z = get_global_id(2);\n";
  c += "  if (X >= dst_size.x || Y >= dst_size.y || Z >= dst_size.z) return;\n";
  c += "  float4 r = (float4)(0.0f);\n";
  c += "  float window_size = 0.0;\n";
  if (stride_correction) {
    c += "  int xs = " +
         GetXStrideCorrected("X", "src_size.w", "stride.x", "padding.x") +
         ";\n";
  } else {
    c += "  int xs = X * stride.x + padding.x;\n";
  }
  c += "  int ys = Y * stride.y + padding.y;\n";
  c += "  for (int ky = 0; ky < kernel_size.y; ++ky) {\n";
  c += "    int y_c = ys + ky;\n";
  c += "    bool outside_y = y_c < 0 || y_c >= src_size.y;\n";
  c += "    for (int kx = 0; kx < kernel_size.x; ++kx) {\n";
  if (op_def.batch_support) {
    c += "      int x_c = xs + kx * src_size.w;\n";
  } else {
    c += "      int x_c = xs + kx;\n";
  }
  c += "      bool outside = outside_y || x_c < 0 || x_c >= src_size.x;\n";
  if (manual_clamp) {
    c += "     r += !outside ? " +
         src_tensor.ReadAsFloat3D("x_c", "y_c", "Z",
                                  TextureAddressMode::DONT_CARE) +
         " : (float4)(0.0f);\n";
  } else {
    c += "      r += " +
         src_tensor.ReadAsFloat3D("x_c", "y_c", "Z", address_mode) + ";\n";
  }
  c += "        window_size += !outside ? 1.0 : 0.0;\n";
  c += "    }\n";
  c += "  }\n";
  // If window_size==0, window covered nothing. This situation is a sign of
  // incorrectly constructed operation. NaNs are expected as output.
  c += "  FLT4 result = TO_FLT4(r / window_size);\n";
  const LinkingContext context{"result", "X", "Y", "Z"};
  c += PostProcess(linked_operations, context);
  c += "  " + dst_tensor.Write3D("result", "X", "Y", "Z");
  c += "}\n";

  return c;
}

std::string GetMaxPoolingKernelCode(
    const OperationDef& op_def, bool stride_correction,
    const std::vector<ElementwiseOperation*>& linked_operations,
    bool output_indices) {
  TensorCodeGenerator src_tensor("src_data",
                                 {"src_size.x", "src_size.y", "src_size.z"},
                                 op_def.src_tensors[0]);
  TensorCodeGenerator dst_tensor("dst_data",
                                 {"dst_size.x", "dst_size.y", "dst_size.z"},
                                 op_def.dst_tensors[0]);
  TensorCodeGenerator indices_tensor("dst_indices",
                                     {"dst_size.x", "dst_size.y", "dst_size.z"},
                                     op_def.dst_tensors[1]);

  std::string c = GetCommonDefines(op_def.precision);

  c += "__kernel void main_function(\n";
  c += src_tensor.GetDeclaration(AccessType::READ);
  c += GetArgsDeclaration(linked_operations);
  c += dst_tensor.GetDeclaration(AccessType::WRITE) + ",\n";
  if (output_indices) {
    c += indices_tensor.GetDeclaration(AccessType::WRITE) + ",\n";
  }
  c += "    int4 src_size,             \n";
  c += "    int4 dst_size,             \n";
  c += "    int2 kernel_size,          \n";
  c += "    int2 padding,              \n";
  c += "    int2 stride                \n";
  c += ") {\n";
  c += "  int X = get_global_id(0);\n";
  c += "  int Y = get_global_id(1);\n";
  c += "  int Z = get_global_id(2);\n";
  c +=
      "  if (X >= dst_size.x || Y >= dst_size.y || Z >= dst_size.z) return; \n";
  c += "  FLT4 maximum = (FLT4)(-10000.0f);\n";
  if (output_indices) {
    c += "  FLT4 indexes = (FLT4)(0.0f);\n";
    c += "  FLT index_counter = (FLT)(0.1f);\n";
  }
  if (stride_correction) {
    c += "  int xs = " +
         GetXStrideCorrected("X", "src_size.w", "stride.x", "padding.x") +
         ";\n";
  } else {
    c += "  int xs = X * stride.x + padding.x;\n";
  }
  c += "  int ys = Y * stride.y + padding.y;\n";
  c += "  for (int ky = 0; ky < kernel_size.y; ++ky) {\n";
  c += "    int y_c = ys + ky;\n";
  c += "    bool outside_y = y_c < 0 || y_c >= src_size.y;\n";
  c += "    for (int kx = 0; kx < kernel_size.x; ++kx) {\n";
  if (op_def.batch_support) {
    c += "      int x_c = xs + kx * src_size.w;\n";
  } else {
    c += "      int x_c = xs + kx;\n";
  }
  c += "      bool outside_x = x_c < 0 || x_c >= src_size.x;\n";
  c += "      if (!outside_x && !outside_y) {\n";
  c += "        FLT4 src = " +
       src_tensor.Read3D("x_c", "y_c", "Z", TextureAddressMode::DONT_CARE) +
       ";\n";
  if (output_indices) {
    c += "        if (src.x > maximum.x) {\n";
    c += "          indexes.x = index_counter;\n";
    c += "          maximum.x = src.x;\n";
    c += "        }\n";
    c += "        if (src.y > maximum.y) {\n";
    c += "          indexes.y = index_counter;\n";
    c += "          maximum.y = src.y;\n";
    c += "        }\n";
    c += "        if (src.z > maximum.z) {\n";
    c += "          indexes.z = index_counter;\n";
    c += "          maximum.z = src.z;\n";
    c += "        }\n";
    c += "        if (src.w > maximum.w) {\n";
    c += "          indexes.w = index_counter;\n";
    c += "          maximum.w = src.w;\n";
    c += "        }\n";
    c += "        index_counter += (FLT)(1.0f);\n";
  }
  c += "        maximum = max(src, maximum);\n";
  c += "      };\n";
  c += "    }\n";
  c += "  }\n";
  const LinkingContext context{"maximum", "X", "Y", "Z"};
  c += PostProcess(linked_operations, context);
  c += "  " + dst_tensor.Write3D("maximum", "X", "Y", "Z");
  if (output_indices) {
    c += "  " + indices_tensor.Write3D("indexes", "X", "Y", "Z");
  }
  c += "}\n";

  return c;
}

}  // namespace

Pooling::Pooling(const OperationDef& definition,
                 const Pooling2DAttributes& attr)
    : GPUOperation(definition),
      stride_(attr.strides.w, attr.strides.h),
      padding_(-attr.padding.prepended.w, -attr.padding.prepended.h),
      kernel_size_(attr.kernel.w, attr.kernel.h),
      type_(attr.type),
      output_indices_(attr.output_indices) {}

Pooling::Pooling(Pooling&& kernel)
    : GPUOperation(std::move(kernel)),
      stride_(kernel.stride_),
      padding_(kernel.padding_),
      kernel_size_(kernel.kernel_size_),
      type_(kernel.type_),
      output_indices_(kernel.output_indices_),
      kernel_(std::move(kernel.kernel_)),
      work_group_size_(kernel.work_group_size_) {}

Pooling& Pooling::operator=(Pooling&& kernel) {
  if (this != &kernel) {
    std::swap(stride_, kernel.stride_);
    std::swap(padding_, kernel.padding_);
    std::swap(kernel_size_, kernel.kernel_size_);
    std::swap(type_, kernel.type_);
    std::swap(output_indices_, kernel.output_indices_);
    kernel_ = std::move(kernel.kernel_);
    std::swap(work_group_size_, kernel.work_group_size_);
    GPUOperation::operator=(std::move(kernel));
  }
  return *this;
}

Status Pooling::Compile(const CreationContext& creation_context) {
  std::string code;
  const bool stride_correction = definition_.batch_support && stride_.x != 1;
  switch (type_) {
    case PoolingType::AVERAGE:
      code = GetAveragePoolingKernelCode(definition_, stride_correction,
                                         *creation_context.device,
                                         linked_operations_);
      break;
    case PoolingType::MAX:
      code = GetMaxPoolingKernelCode(definition_, stride_correction,
                                     linked_operations_, output_indices_);
      break;
    default:
      return InvalidArgumentError(
          "You should create another kernel with this params");
      break;
  }
  return creation_context.cache->GetOrCreateCLKernel(
      code, "main_function", *creation_context.context,
      *creation_context.device, &kernel_);
}

Status Pooling::BindArguments() {
  kernel_.ResetBindingCounter();
  RETURN_IF_ERROR(kernel_.SetMemoryAuto(src_[0]->GetMemoryPtr()));
  RETURN_IF_ERROR(BindArgs(&kernel_, linked_operations_));
  RETURN_IF_ERROR(kernel_.SetMemoryAuto(dst_[0]->GetMemoryPtrForWriting()));
  if (output_indices_) {
    RETURN_IF_ERROR(kernel_.SetMemoryAuto(dst_[1]->GetMemoryPtrForWriting()));
  }
  RETURN_IF_ERROR(kernel_.SetBytesAuto(src_[0]->GetWBatchedHDB()));
  RETURN_IF_ERROR(kernel_.SetBytesAuto(dst_[0]->GetWBatchedHDB()));
  RETURN_IF_ERROR(kernel_.SetBytesAuto(kernel_size_));
  RETURN_IF_ERROR(
      kernel_.SetBytesAuto(int2(padding_.x * src_[0]->Batch(), padding_.y)));
  RETURN_IF_ERROR(kernel_.SetBytesAuto(stride_));

  return OkStatus();
}

int3 Pooling::GetGridSize() const {
  const int grid_x = dst_[0]->Width() * dst_[0]->Batch();
  const int grid_y = dst_[0]->Height();
  const int grid_z = dst_[0]->Depth();
  return int3(grid_x, grid_y, grid_z);
}

Status Pooling::Tune(const TuningParameters& params) {
  RETURN_IF_ERROR(BindArguments());
  return GetBestWorkGroup(params, kernel_, GetGridSize(), &work_group_size_);
}

Status Pooling::AddToQueue(CLCommandQueue* queue) {
  RETURN_IF_ERROR(BindArguments());
  return queue->DispatchImplicit(kernel_, GetGridSize(), work_group_size_);
}

Pooling CreatePooling(const OperationDef& definition,
                      const Pooling2DAttributes& attr) {
  return Pooling(definition, attr);
}

}  // namespace cl
}  // namespace gpu
}  // namespace tflite
<|endoftext|>


<commit_before>

#include "Globals.h"

#include "IncrementalRedstoneSimulator.h"
#include "Chunk.h"

#include "CommandBlockHandler.h"
#include "DoorHandler.h"
#include "RedstoneHandler.h"
#include "RedstoneTorchHandler.h"
#include "RedstoneWireHandler.h"
#include "RedstoneRepeaterHandler.h"
#include "RedstoneToggleHandler.h"
#include "SolidBlockHandler.h"
#include "RedstoneLampHandler.h"
#include "RedstoneBlockHandler.h"
#include "PistonHandler.h"
#include "SmallGateHandler.h"
#include "NoteBlockHandler.h"
#include "TNTHandler.h"
#include "PoweredRailHandler.h"
#include "PressurePlateHandler.h"
#include "TripwireHookHandler.h"
#include "DropSpenserHandler.h"
#include "RedstoneComparatorHandler.h"
#include "TrappedChestHandler.h"

std::unique_ptr<cRedstoneHandler> cIncrementalRedstoneSimulator::CreateComponent(cWorld & a_World, BLOCKTYPE a_BlockType, cIncrementalRedstoneSimulatorChunkData * a_Data)
{
	switch (a_BlockType)
	{
		case E_BLOCK_ACTIVATOR_RAIL:
		case E_BLOCK_DETECTOR_RAIL:
		case E_BLOCK_POWERED_RAIL: return cpp14::make_unique<cPoweredRailHandler>(a_World);

		case E_BLOCK_ACTIVE_COMPARATOR:
		case E_BLOCK_INACTIVE_COMPARATOR: return cpp14::make_unique<cRedstoneComparatorHandler>(a_World);

		case E_BLOCK_DISPENSER:
		case E_BLOCK_DROPPER: return cpp14::make_unique<cDropSpenserHandler>(a_World);

		case E_BLOCK_HEAVY_WEIGHTED_PRESSURE_PLATE:
		case E_BLOCK_LIGHT_WEIGHTED_PRESSURE_PLATE:
		case E_BLOCK_STONE_PRESSURE_PLATE:
		case E_BLOCK_WOODEN_PRESSURE_PLATE: return cpp14::make_unique<cPressurePlateHandler>(a_World);

		case E_BLOCK_FENCE_GATE:
		case E_BLOCK_IRON_TRAPDOOR:
		case E_BLOCK_TRAPDOOR: return cpp14::make_unique<cSmallGateHandler>(a_World);

		case E_BLOCK_REDSTONE_LAMP_OFF:
		case E_BLOCK_REDSTONE_LAMP_ON: return cpp14::make_unique<cRedstoneLampHandler>(a_World);

		case E_BLOCK_REDSTONE_REPEATER_OFF:
		case E_BLOCK_REDSTONE_REPEATER_ON: return cpp14::make_unique<cRedstoneRepeaterHandler>(a_World);

		case E_BLOCK_REDSTONE_TORCH_OFF:
		case E_BLOCK_REDSTONE_TORCH_ON: return cpp14::make_unique<cRedstoneTorchHandler>(a_World);

		case E_BLOCK_PISTON:
		case E_BLOCK_STICKY_PISTON: return cpp14::make_unique<cPistonHandler>(a_World);

		case E_BLOCK_LEVER:
		case E_BLOCK_STONE_BUTTON:
		case E_BLOCK_WOODEN_BUTTON: return cpp14::make_unique<cRedstoneToggleHandler>(a_World);

		case E_BLOCK_BLOCK_OF_REDSTONE: return cpp14::make_unique<cRedstoneBlockHandler>(a_World);
		case E_BLOCK_COMMAND_BLOCK: return cpp14::make_unique<cCommandBlockHandler>(a_World);
		case E_BLOCK_NOTE_BLOCK: return cpp14::make_unique<cNoteBlockHandler>(a_World);
		case E_BLOCK_REDSTONE_WIRE: return cpp14::make_unique<cRedstoneWireHandler>(a_World);
		case E_BLOCK_TNT: return cpp14::make_unique<cTNTHandler>(a_World);
		case E_BLOCK_TRAPPED_CHEST: return cpp14::make_unique<cTrappedChestHandler>(a_World);
		case E_BLOCK_TRIPWIRE_HOOK: return cpp14::make_unique<cTripwireHookHandler>(a_World);
		default:
		{
			if (cBlockDoorHandler::IsDoorBlockType(a_BlockType))
			{
				return cpp14::make_unique<cDoorHandler>(a_World);
			}

			if (cBlockInfo::FullyOccupiesVoxel(a_BlockType))
			{
				return cpp14::make_unique<cSolidBlockHandler>(a_World);
			}
			return nullptr;
		}
	}
}

void cIncrementalRedstoneSimulator::Simulate(float a_dt)
{
	for (auto & DelayInfo : m_Data.m_MechanismDelays)
	{
		if ((--DelayInfo.second.first) == 0)
		{
			m_Data.GetActiveBlocks().emplace_back(DelayInfo.first);
		}
	}

	// Build our work queue
	cVector3iArray WorkQueue;
	std::swap(WorkQueue, m_Data.GetActiveBlocks());

	// Process the work queue
	while (!WorkQueue.empty())
	{
		// Grab the first element and remove it from the list
		Vector3i CurrentLocation = WorkQueue.back();
		WorkQueue.pop_back();

		BLOCKTYPE CurrentBlock;
		NIBBLETYPE CurrentMeta;
		if (!m_World.GetBlockTypeMeta(CurrentLocation.x, CurrentLocation.y, CurrentLocation.z, CurrentBlock, CurrentMeta))
		{
			continue;
		}

		auto CurrentHandler = cIncrementalRedstoneSimulator::CreateComponent(m_World, CurrentBlock, &m_Data);
		if (CurrentHandler == nullptr)  // Block at CurrentPosition doesn't have a corresponding redstone handler
		{
			// Clean up cached PowerData for CurrentPosition
			static_cast<cIncrementalRedstoneSimulator *>(m_World.GetRedstoneSimulator())->GetChunkData()->ErasePowerData(CurrentLocation);

			continue;
		}

		cRedstoneHandler::PoweringData Power;
		for (const auto & Location : CurrentHandler->GetValidSourcePositions(CurrentLocation, CurrentBlock, CurrentMeta))
		{
			if (!cChunk::IsValidHeight(Location.y))
			{
				continue;
			}
			BLOCKTYPE PotentialBlock;
			NIBBLETYPE PotentialMeta;
			m_World.GetBlockTypeMeta(Location.x, Location.y, Location.z, PotentialBlock, PotentialMeta);

			auto PotentialSourceHandler = cIncrementalRedstoneSimulator::CreateComponent(m_World, PotentialBlock, &m_Data);
			if (PotentialSourceHandler == nullptr)
			{
				continue;
			}

			decltype(Power) PotentialPower(PotentialBlock, PotentialSourceHandler->GetPowerDeliveredToPosition(Location, PotentialBlock, PotentialMeta, CurrentLocation, CurrentBlock));
			Power = std::max(Power, PotentialPower);
		}

		// Inform the handler to update
		cVector3iArray Updates = CurrentHandler->Update(CurrentLocation, CurrentBlock, CurrentMeta, Power);
		WorkQueue.insert(WorkQueue.end(), Updates.begin(), Updates.end());

		if (IsAlwaysTicked(CurrentBlock))
		{
			m_Data.GetActiveBlocks().emplace_back(CurrentLocation);
		}
	}
}
<commit_msg>Fix fence gates (#3683)<commit_after>

#include "Globals.h"

#include "IncrementalRedstoneSimulator.h"
#include "Chunk.h"

#include "CommandBlockHandler.h"
#include "DoorHandler.h"
#include "RedstoneHandler.h"
#include "RedstoneTorchHandler.h"
#include "RedstoneWireHandler.h"
#include "RedstoneRepeaterHandler.h"
#include "RedstoneToggleHandler.h"
#include "SolidBlockHandler.h"
#include "RedstoneLampHandler.h"
#include "RedstoneBlockHandler.h"
#include "PistonHandler.h"
#include "SmallGateHandler.h"
#include "NoteBlockHandler.h"
#include "TNTHandler.h"
#include "PoweredRailHandler.h"
#include "PressurePlateHandler.h"
#include "TripwireHookHandler.h"
#include "DropSpenserHandler.h"
#include "RedstoneComparatorHandler.h"
#include "TrappedChestHandler.h"

std::unique_ptr<cRedstoneHandler> cIncrementalRedstoneSimulator::CreateComponent(cWorld & a_World, BLOCKTYPE a_BlockType, cIncrementalRedstoneSimulatorChunkData * a_Data)
{
	switch (a_BlockType)
	{
		case E_BLOCK_ACTIVATOR_RAIL:
		case E_BLOCK_DETECTOR_RAIL:
		case E_BLOCK_POWERED_RAIL: return cpp14::make_unique<cPoweredRailHandler>(a_World);

		case E_BLOCK_ACTIVE_COMPARATOR:
		case E_BLOCK_INACTIVE_COMPARATOR: return cpp14::make_unique<cRedstoneComparatorHandler>(a_World);

		case E_BLOCK_DISPENSER:
		case E_BLOCK_DROPPER: return cpp14::make_unique<cDropSpenserHandler>(a_World);

		case E_BLOCK_HEAVY_WEIGHTED_PRESSURE_PLATE:
		case E_BLOCK_LIGHT_WEIGHTED_PRESSURE_PLATE:
		case E_BLOCK_STONE_PRESSURE_PLATE:
		case E_BLOCK_WOODEN_PRESSURE_PLATE: return cpp14::make_unique<cPressurePlateHandler>(a_World);

		case E_BLOCK_ACACIA_FENCE_GATE:
		case E_BLOCK_BIRCH_FENCE_GATE:
		case E_BLOCK_DARK_OAK_FENCE_GATE:
		case E_BLOCK_FENCE_GATE:
		case E_BLOCK_IRON_TRAPDOOR:
		case E_BLOCK_JUNGLE_FENCE_GATE:
		case E_BLOCK_SPRUCE_FENCE_GATE:
		case E_BLOCK_TRAPDOOR: return cpp14::make_unique<cSmallGateHandler>(a_World);

		case E_BLOCK_REDSTONE_LAMP_OFF:
		case E_BLOCK_REDSTONE_LAMP_ON: return cpp14::make_unique<cRedstoneLampHandler>(a_World);

		case E_BLOCK_REDSTONE_REPEATER_OFF:
		case E_BLOCK_REDSTONE_REPEATER_ON: return cpp14::make_unique<cRedstoneRepeaterHandler>(a_World);

		case E_BLOCK_REDSTONE_TORCH_OFF:
		case E_BLOCK_REDSTONE_TORCH_ON: return cpp14::make_unique<cRedstoneTorchHandler>(a_World);

		case E_BLOCK_PISTON:
		case E_BLOCK_STICKY_PISTON: return cpp14::make_unique<cPistonHandler>(a_World);

		case E_BLOCK_LEVER:
		case E_BLOCK_STONE_BUTTON:
		case E_BLOCK_WOODEN_BUTTON: return cpp14::make_unique<cRedstoneToggleHandler>(a_World);

		case E_BLOCK_BLOCK_OF_REDSTONE: return cpp14::make_unique<cRedstoneBlockHandler>(a_World);
		case E_BLOCK_COMMAND_BLOCK: return cpp14::make_unique<cCommandBlockHandler>(a_World);
		case E_BLOCK_NOTE_BLOCK: return cpp14::make_unique<cNoteBlockHandler>(a_World);
		case E_BLOCK_REDSTONE_WIRE: return cpp14::make_unique<cRedstoneWireHandler>(a_World);
		case E_BLOCK_TNT: return cpp14::make_unique<cTNTHandler>(a_World);
		case E_BLOCK_TRAPPED_CHEST: return cpp14::make_unique<cTrappedChestHandler>(a_World);
		case E_BLOCK_TRIPWIRE_HOOK: return cpp14::make_unique<cTripwireHookHandler>(a_World);
		default:
		{
			if (cBlockDoorHandler::IsDoorBlockType(a_BlockType))
			{
				return cpp14::make_unique<cDoorHandler>(a_World);
			}

			if (cBlockInfo::FullyOccupiesVoxel(a_BlockType))
			{
				return cpp14::make_unique<cSolidBlockHandler>(a_World);
			}
			return nullptr;
		}
	}
}

void cIncrementalRedstoneSimulator::Simulate(float a_dt)
{
	for (auto & DelayInfo : m_Data.m_MechanismDelays)
	{
		if ((--DelayInfo.second.first) == 0)
		{
			m_Data.GetActiveBlocks().emplace_back(DelayInfo.first);
		}
	}

	// Build our work queue
	cVector3iArray WorkQueue;
	std::swap(WorkQueue, m_Data.GetActiveBlocks());

	// Process the work queue
	while (!WorkQueue.empty())
	{
		// Grab the first element and remove it from the list
		Vector3i CurrentLocation = WorkQueue.back();
		WorkQueue.pop_back();

		BLOCKTYPE CurrentBlock;
		NIBBLETYPE CurrentMeta;
		if (!m_World.GetBlockTypeMeta(CurrentLocation.x, CurrentLocation.y, CurrentLocation.z, CurrentBlock, CurrentMeta))
		{
			continue;
		}

		auto CurrentHandler = cIncrementalRedstoneSimulator::CreateComponent(m_World, CurrentBlock, &m_Data);
		if (CurrentHandler == nullptr)  // Block at CurrentPosition doesn't have a corresponding redstone handler
		{
			// Clean up cached PowerData for CurrentPosition
			static_cast<cIncrementalRedstoneSimulator *>(m_World.GetRedstoneSimulator())->GetChunkData()->ErasePowerData(CurrentLocation);

			continue;
		}

		cRedstoneHandler::PoweringData Power;
		for (const auto & Location : CurrentHandler->GetValidSourcePositions(CurrentLocation, CurrentBlock, CurrentMeta))
		{
			if (!cChunk::IsValidHeight(Location.y))
			{
				continue;
			}
			BLOCKTYPE PotentialBlock;
			NIBBLETYPE PotentialMeta;
			m_World.GetBlockTypeMeta(Location.x, Location.y, Location.z, PotentialBlock, PotentialMeta);

			auto PotentialSourceHandler = cIncrementalRedstoneSimulator::CreateComponent(m_World, PotentialBlock, &m_Data);
			if (PotentialSourceHandler == nullptr)
			{
				continue;
			}

			decltype(Power) PotentialPower(PotentialBlock, PotentialSourceHandler->GetPowerDeliveredToPosition(Location, PotentialBlock, PotentialMeta, CurrentLocation, CurrentBlock));
			Power = std::max(Power, PotentialPower);
		}

		// Inform the handler to update
		cVector3iArray Updates = CurrentHandler->Update(CurrentLocation, CurrentBlock, CurrentMeta, Power);
		WorkQueue.insert(WorkQueue.end(), Updates.begin(), Updates.end());

		if (IsAlwaysTicked(CurrentBlock))
		{
			m_Data.GetActiveBlocks().emplace_back(CurrentLocation);
		}
	}
}
<|endoftext|>


<commit_before>/*
 * Copyright 2017-present Facebook, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <thrift/lib/cpp2/transport/util/ConnectionThread.h>

#include <folly/portability/GFlags.h>
#include <glog/logging.h>

#include <folly/Conv.h>
#include <thrift/lib/cpp/async/TAsyncSSLSocket.h>
#include <thrift/lib/cpp/async/TAsyncSocket.h>
#include <thrift/lib/cpp/async/TAsyncTransport.h>
#include <thrift/lib/cpp2/transport/http2/client/H2ClientConnection.h>

DEFINE_string(transport, "http2", "The transport to use (http2)");
DEFINE_bool(use_ssl, false, "Create an encrypted client connection");

namespace apache {
namespace thrift {

using apache::thrift::async::TAsyncSocket;
using apache::thrift::async::TAsyncSSLSocket;
using apache::thrift::async::TAsyncTransport;

ConnectionThread::~ConnectionThread() {
  getEventBase()->runInEventBaseThreadAndWait(
      [&] { connections_.wlock()->clear(); });
}

std::shared_ptr<ClientConnectionIf> ConnectionThread::getConnection(
    const std::string& addr,
    uint16_t port) {
  std::string serverKey = folly::to<std::string>(addr, ":", port);
  getEventBase()->runInEventBaseThreadAndWait(
      [&]() { maybeCreateConnection(serverKey, addr, port); });
  return connections_.withWLock(
      [&](auto& connections) { return connections[serverKey]; });
}

void ConnectionThread::maybeCreateConnection(
    const std::string& serverKey,
    const std::string& addr,
    uint16_t port) {
  LOG_IF(FATAL, FLAGS_transport == "rsocket" || FLAGS_transport == "rocket")
      << "Use RSocketClientChannel::newChannel() or"
         " RocketClientChannel::newChannel()";

  connections_.withWLock([&, this](auto& connections) {
    std::shared_ptr<ClientConnectionIf>& connection = connections[serverKey];
    if (connection == nullptr || !connection->good()) {
      TAsyncSocket::UniquePtr socket(
          new TAsyncSocket(this->getEventBase(), addr, port));
      if (FLAGS_use_ssl) {
        auto sslContext = std::make_shared<folly::SSLContext>();
        sslContext->setAdvertisedNextProtocols({"h2", "http"});
        auto sslSocket = new TAsyncSSLSocket(
            sslContext,
            this->getEventBase(),
            socket->detachNetworkSocket().toFd(),
            false);
        sslSocket->sslConn(nullptr);
        socket.reset(sslSocket);
      }
      if (FLAGS_transport != "http2") {
        LOG(ERROR) << "Unknown transport " << FLAGS_transport
                   << ".  Will use http2.";
      }
      connection = H2ClientConnection::newHTTP2Connection(std::move(socket));
    }
  });
}

} // namespace thrift
} // namespace apache
<commit_msg>Don't call setAdvertisedNextProtocols() if OpenSSL is too old<commit_after>/*
 * Copyright 2017-present Facebook, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <thrift/lib/cpp2/transport/util/ConnectionThread.h>

#include <folly/portability/GFlags.h>
#include <glog/logging.h>

#include <folly/Conv.h>
#include <thrift/lib/cpp/async/TAsyncSSLSocket.h>
#include <thrift/lib/cpp/async/TAsyncSocket.h>
#include <thrift/lib/cpp/async/TAsyncTransport.h>
#include <thrift/lib/cpp2/transport/http2/client/H2ClientConnection.h>

DEFINE_string(transport, "http2", "The transport to use (http2)");
DEFINE_bool(use_ssl, false, "Create an encrypted client connection");

namespace apache {
namespace thrift {

using apache::thrift::async::TAsyncSocket;
using apache::thrift::async::TAsyncSSLSocket;
using apache::thrift::async::TAsyncTransport;

ConnectionThread::~ConnectionThread() {
  getEventBase()->runInEventBaseThreadAndWait(
      [&] { connections_.wlock()->clear(); });
}

std::shared_ptr<ClientConnectionIf> ConnectionThread::getConnection(
    const std::string& addr,
    uint16_t port) {
  std::string serverKey = folly::to<std::string>(addr, ":", port);
  getEventBase()->runInEventBaseThreadAndWait(
      [&]() { maybeCreateConnection(serverKey, addr, port); });
  return connections_.withWLock(
      [&](auto& connections) { return connections[serverKey]; });
}

void ConnectionThread::maybeCreateConnection(
    const std::string& serverKey,
    const std::string& addr,
    uint16_t port) {
  LOG_IF(FATAL, FLAGS_transport == "rsocket" || FLAGS_transport == "rocket")
      << "Use RSocketClientChannel::newChannel() or"
         " RocketClientChannel::newChannel()";

  connections_.withWLock([&, this](auto& connections) {
    std::shared_ptr<ClientConnectionIf>& connection = connections[serverKey];
    if (connection == nullptr || !connection->good()) {
      TAsyncSocket::UniquePtr socket(
          new TAsyncSocket(this->getEventBase(), addr, port));
      if (FLAGS_use_ssl) {
        auto sslContext = std::make_shared<folly::SSLContext>();
#if FOLLY_OPENSSL_HAS_ALPN
        sslContext->setAdvertisedNextProtocols({"h2", "http"});
#endif
        auto sslSocket = new TAsyncSSLSocket(
            sslContext,
            this->getEventBase(),
            socket->detachNetworkSocket().toFd(),
            false);
        sslSocket->sslConn(nullptr);
        socket.reset(sslSocket);
      }
      if (FLAGS_transport != "http2") {
        LOG(ERROR) << "Unknown transport " << FLAGS_transport
                   << ".  Will use http2.";
      }
      connection = H2ClientConnection::newHTTP2Connection(std::move(socket));
    }
  });
}

} // namespace thrift
} // namespace apache
<|endoftext|>


<commit_before>/*
Uses the sound_play library of ROS to play sound on the OS' default sound driver.
Listening on:
	/tawi/motors/wheels to geometry_msgs::Twist (directions to drive)
Publishing on:

*/
#include <threemxl/platform/io/configuration/XMLConfiguration.h>
#include <threemxl/dxlassert.h>
#include <ros/ros.h>
#include <threemxl/C3mxlROS.h>
#include <geometry_msgs/Twist.h>
#include "drivingDriver.h"

DrivingDriver::DrivingDriver() {
	DrivingDriver(99999);
}

DrivingDriver::DrivingDriver(int id){
	ros::NodeHandle handle("~");
	ID = id;
	sub = handle.subscribe<geometry_msgs::Twist>("/base/cmd_vel", 10, &DrivingDriver::driveCallback, this);

	//Stolen code to set some things. No idea where to find the parameters...

	ROS_ASSERT(handle.getParam("motor_port", motor_port_name));
	ROS_ASSERT(handle.getParam("motor_config", motor_config_name));
	ROS_ASSERT(handle.getParam("wheel_diameter", wheelDiameter));
	ROS_ASSERT(handle.getParam("wheel_base", wheelBase));
/*

	motor_port_name = "motor_comm";
	motor_config_name = "wheels.xml";
	wheelDiameter = 0.295;
	wheelBase = 0.54;
*/
	ROS_ASSERT(motor_config_xml.loadFile(motor_config_name));

	//End of stolen code

	initMotors();

	DXL_SAFE_CALL(left_motor->set3MxlMode(motor_config_left.m3mxlMode));
	DXL_SAFE_CALL(right_motor->set3MxlMode(motor_config_right.m3mxlMode));
}

void DrivingDriver::driveCallback(const geometry_msgs::Twist::ConstPtr &msg){

	ROS_INFO("driveCallback");
	double v_x = msg->linear.x/(wheelDiameter/2);
	double v_theta = msg->angular.z * (wheelBase/wheelDiameter);

	double leftMotorVelocity = v_x - v_theta;
	double rightMotorVelocity = v_x + v_theta;

	left_motor->set3MxlMode(SPEED_MODE);
	left_motor->setSpeed(leftMotorVelocity);
	right_motor->set3MxlMode(SPEED_MODE);
	right_motor->setSpeed(rightMotorVelocity);

}

int DrivingDriver::getID() {
	return ID;
}

int DrivingDriver::getState(){
	return state;
}

bool DrivingDriver::setPower(int level){
	currentPower = level;
}

int DrivingDriver::getPower(){
	return currentPower;
}

bool DrivingDriver::drive(){

	return true;
}

bool DrivingDriver::stop() {
	//dingen
	return false;
}

void DrivingDriver::initMotors(){
	ros::Rate init_rate(1);

	motor_config_left.readConfig(motor_config_xml.root().section("left"));
	left_motor = new C3mxlROS(motor_port_name.c_str());
	left_motor->setConfig(&motor_config_left);

	while (ros::ok() && left_motor->init() != DXL_SUCCESS) {
		ROS_WARN_ONCE("Couldn't initialize left wheel motor, will continue trying every second");
		init_rate.sleep();
	}

	motor_config_right.readConfig(motor_config_xml.root().section("right"));
	right_motor = new C3mxlROS(motor_port_name.c_str());
	right_motor->setConfig(&motor_config_right);

	while (ros::ok() && right_motor->init() != DXL_SUCCESS) {
		ROS_WARN_ONCE("Couldn't initialize right wheel motor, will continue trying every second");
		init_rate.sleep();
	}
}

void DrivingDriver::spin() {
	ROS_INFO("Spinning");

	ros::Rate r(1000);

	while(ros::ok()) {
		ros::spinOnce();
		r.sleep();
	}
}

int main(int argc, char **argv)
{

	ros::init(argc, argv, "drivingDriver");
	DrivingDriver dd;
	dd.spin();

	return 0;
}<commit_msg>Minor cleanup in drivingDriver.cpp<commit_after>/*
Uses the sound_play library of ROS to play sound on the OS' default sound driver.
Listening on:
	/tawi/motors/wheels to geometry_msgs::Twist (directions to drive)
Publishing on:

*/
#include <threemxl/platform/io/configuration/XMLConfiguration.h>
#include <threemxl/dxlassert.h>
#include <ros/ros.h>
#include <threemxl/C3mxlROS.h>
#include <geometry_msgs/Twist.h>
#include "drivingDriver.h"

DrivingDriver::DrivingDriver() {
	DrivingDriver(99999);
}

DrivingDriver::DrivingDriver(int id){
	ros::NodeHandle handle("~");
	ID = id;
	sub = handle.subscribe<geometry_msgs::Twist>("/base/cmd_vel", 10, &DrivingDriver::driveCallback, this);

	//Parameters set in launch file.
	ROS_ASSERT(handle.getParam("motor_port", motor_port_name));
	ROS_ASSERT(handle.getParam("motor_config", motor_config_name));
	ROS_ASSERT(handle.getParam("wheel_diameter", wheelDiameter));
	ROS_ASSERT(handle.getParam("wheel_base", wheelBase));

	ROS_ASSERT(motor_config_xml.loadFile(motor_config_name));

	initMotors();

	DXL_SAFE_CALL(left_motor->set3MxlMode(motor_config_left.m3mxlMode));
	DXL_SAFE_CALL(right_motor->set3MxlMode(motor_config_right.m3mxlMode));
}

void DrivingDriver::driveCallback(const geometry_msgs::Twist::ConstPtr &msg){

	ROS_INFO("driveCallback");
	double v_x = msg->linear.x/(wheelDiameter/2);
	double v_theta = msg->angular.z * (wheelBase/wheelDiameter);

	double leftMotorVelocity = v_x - v_theta;
	double rightMotorVelocity = v_x + v_theta;

	left_motor->set3MxlMode(SPEED_MODE);
	left_motor->setSpeed(leftMotorVelocity);
	right_motor->set3MxlMode(SPEED_MODE);
	right_motor->setSpeed(rightMotorVelocity);

}

int DrivingDriver::getID() {
	return ID;
}

int DrivingDriver::getState(){
	return state;
}

bool DrivingDriver::setPower(int level){
	currentPower = level;
}

int DrivingDriver::getPower(){
	return currentPower;
}

bool DrivingDriver::drive(){

	return true;
}

bool DrivingDriver::stop() {
	//dingen
	return false;
}

void DrivingDriver::initMotors(){
	ros::Rate init_rate(1);

	motor_config_left.readConfig(motor_config_xml.root().section("left"));
	left_motor = new C3mxlROS(motor_port_name.c_str());
	left_motor->setConfig(&motor_config_left);

	while (ros::ok() && left_motor->init() != DXL_SUCCESS) {
		ROS_WARN_ONCE("Couldn't initialize left wheel motor, will continue trying every second");
		init_rate.sleep();
	}

	motor_config_right.readConfig(motor_config_xml.root().section("right"));
	right_motor = new C3mxlROS(motor_port_name.c_str());
	right_motor->setConfig(&motor_config_right);

	while (ros::ok() && right_motor->init() != DXL_SUCCESS) {
		ROS_WARN_ONCE("Couldn't initialize right wheel motor, will continue trying every second");
		init_rate.sleep();
	}
}

void DrivingDriver::spin() {
	ROS_INFO("Spinning");

	ros::Rate r(1000);

	while(ros::ok()) {
		ros::spinOnce();
		r.sleep();
	}
}

int main(int argc, char **argv)
{

	ros::init(argc, argv, "drivingDriver");
	DrivingDriver dd;
	dd.spin();

	return 0;
}<|endoftext|>


<commit_before>// See Fossen 2011, chapter 12.3.2
#include "pseudoinverse_allocator.h"

template<typename Derived>
inline bool isFucked(const Eigen::MatrixBase<Derived>& x)
{
  return !((x.array() == x.array())).all() && !( (x - x).array() == (x - x).array()).all();
}

PseudoinverseAllocator::PseudoinverseAllocator()
{
  sub = nh.subscribe("rov_forces", 10, &PseudoinverseAllocator::callback, this);
  pub = nh.advertise<vortex_msgs::ThrusterForces>("thruster_forces", 10);

  W.setIdentity(); // Default to identity (i.e. equal weights)
  K.setIdentity(); // Scaling is done on Arduino, so this can be identity

  // Thruster configuration does not allow control of roll.
  T <<  0     ,  0.7071, -0.7071, 0     ,  0.7071, -0.7071, // Surge
        0     , -0.7071, -0.7071, 0     ,  0.7071,  0.7071, // Sway
        1     ,  0     ,  0     , 1     ,  0     ,  0     , // Heave
       -0.1600, -0.0467,  0.0467, 0.1600, -0.0467,  0.0467, // Pitch
        0     , -0.2143,  0.2143, 0     ,  0.2143, -0.2143; // Yaw

  K_inverse = K.inverse();
  computePseudoinverse();
}

void PseudoinverseAllocator::callback(const geometry_msgs::Wrench& tauMsg)
{
  tau << tauMsg.force.x,
         tauMsg.force.y,
         tauMsg.force.z,
         tauMsg.torque.y,
         tauMsg.torque.z;

  u = K_inverse * T_pseudoinverse * tau;

  if (isFucked(K_inverse))
    ROS_WARN("K is not invertible");

  vortex_msgs::ThrusterForces uMsg;
  uMsg.F1 = u(0);
  uMsg.F2 = u(1);
  uMsg.F3 = u(2);
  uMsg.F4 = u(3);
  uMsg.F5 = u(4);
  uMsg.F6 = u(5);
  pub.publish(uMsg);
  ROS_INFO("pseudoinverse_allocator: Publishing thruster forces.");
}

void PseudoinverseAllocator::setWeights(const Eigen::MatrixXd &W_new)
{
  bool isCorrectDimensions = ( W_new.rows() == r && W_new.cols() == r );
  if (!isCorrectDimensions)
  {
    ROS_WARN_STREAM("Attempt to set weight matrix in PseudoinverseAllocator with wrong dimensions " << W_new.rows() << "*" << W_new.cols() << ".");
    return;
  }

  W = W_new;

  // New weights require recomputing the generalized inverse of the thrust config matrix
  computePseudoinverse();
}

void PseudoinverseAllocator::computePseudoinverse()
{
  T_pseudoinverse = W.inverse()*T.transpose() * (T*W.inverse()*T.transpose()).inverse();

  if (isFucked(T_pseudoinverse))
    ROS_WARN("NaN in T_pseudoinverse.");
  if (isFucked(W.inverse()))
    ROS_WARN("W not invertible.");
  if (isFucked((T*W.inverse()*T.transpose()).inverse()))
    ROS_WARN("T * W_inv * T transposed is not invertible.");
}
<commit_msg>Add yaml matrix parser<commit_after>// See Fossen 2011, chapter 12.3.2
#include "pseudoinverse_allocator.h"

template<typename Derived>
inline bool isFucked(const Eigen::MatrixBase<Derived>& x)
{
  return !((x.array() == x.array())).all() && !( (x - x).array() == (x - x).array()).all();
}

Eigen::MatrixXd parseYamlMatrix(ros::NodeHandle nh, std::string matrixName)
{
  // TODO: Don't use asserts like this, instead check for validity first and return something if invalid.

  XmlRpc::XmlRpcValue matrix;
  nh.getParam(matrixName, matrix);

    // The matrix will be an array of arrays. Thus the "outer" data type should be an array.
  ROS_ASSERT(matrix.getType() == XmlRpc::XmlRpcValue::TypeArray);

  const int rows = matrix.size();
  const int cols = matrix[0].size();
  Eigen::MatrixXd X(rows,cols);

  for (int i = 0; i < matrix.size(); ++i)
  {
        // Each row should also be an array.
    ROS_ASSERT(matrix[i].getType() == XmlRpc::XmlRpcValue::TypeArray);

    for (int j = 0; j < matrix[i].size(); ++j)
    {
            // Each element should be a double (maybe not require this?)
      ROS_ASSERT(matrix[i][j].getType() == XmlRpc::XmlRpcValue::TypeDouble);
      X(i,j) = matrix[i][j];
    }
  }

  return X;
}

PseudoinverseAllocator::PseudoinverseAllocator()
{
  sub = nh.subscribe("rov_forces", 10, &PseudoinverseAllocator::callback, this);
  pub = nh.advertise<vortex_msgs::ThrusterForces>("thruster_forces", 10);

  W.setIdentity(); // Default to identity (i.e. equal weights)
  K.setIdentity(); // Scaling is done on Arduino, so this can be identity

  // Thruster configuration does not allow control of roll.
  T <<  0     ,  0.7071, -0.7071, 0     ,  0.7071, -0.7071, // Surge
        0     , -0.7071, -0.7071, 0     ,  0.7071,  0.7071, // Sway
        1     ,  0     ,  0     , 1     ,  0     ,  0     , // Heave
       -0.1600, -0.0467,  0.0467, 0.1600, -0.0467,  0.0467, // Pitch
        0     , -0.2143,  0.2143, 0     ,  0.2143, -0.2143; // Yaw

  K_inverse = K.inverse();
  computePseudoinverse();
}

void PseudoinverseAllocator::callback(const geometry_msgs::Wrench& tauMsg)
{
  tau << tauMsg.force.x,
         tauMsg.force.y,
         tauMsg.force.z,
         tauMsg.torque.y,
         tauMsg.torque.z;

  u = K_inverse * T_pseudoinverse * tau;

  if (isFucked(K_inverse))
    ROS_WARN("K is not invertible");

  vortex_msgs::ThrusterForces uMsg;
  uMsg.F1 = u(0);
  uMsg.F2 = u(1);
  uMsg.F3 = u(2);
  uMsg.F4 = u(3);
  uMsg.F5 = u(4);
  uMsg.F6 = u(5);
  pub.publish(uMsg);
  ROS_INFO("pseudoinverse_allocator: Publishing thruster forces.");
}

void PseudoinverseAllocator::setWeights(const Eigen::MatrixXd &W_new)
{
  bool isCorrectDimensions = ( W_new.rows() == r && W_new.cols() == r );
  if (!isCorrectDimensions)
  {
    ROS_WARN_STREAM("Attempt to set weight matrix in PseudoinverseAllocator with wrong dimensions " << W_new.rows() << "*" << W_new.cols() << ".");
    return;
  }

  W = W_new;

  // New weights require recomputing the generalized inverse of the thrust config matrix
  computePseudoinverse();
}

void PseudoinverseAllocator::computePseudoinverse()
{
  T_pseudoinverse = W.inverse()*T.transpose() * (T*W.inverse()*T.transpose()).inverse();

  if (isFucked(T_pseudoinverse))
    ROS_WARN("NaN in T_pseudoinverse.");
  if (isFucked(W.inverse()))
    ROS_WARN("W not invertible.");
  if (isFucked((T*W.inverse()*T.transpose()).inverse()))
    ROS_WARN("T * W_inv * T transposed is not invertible.");
}
<|endoftext|>


<commit_before>void RICHtest (Int_t evNumber1=0,Int_t evNumber2=0)
{
/////////////////////////////////////////////////////////////////////////
//   This macro is a small example of a ROOT macro
//   illustrating how to read the output of GALICE
//   and do some analysis.
//
/////////////////////////////////////////////////////////////////////////

// Dynamically link some shared libs

    if (gClassTable->GetID("AliRun") < 0) {
	gROOT->LoadMacro("loadlibs.C");
	loadlibs();
    }
// Connect the Root Galice file containing Geometry, Kine and Hits

    TFile *file = (TFile*)gROOT->GetListOfFiles()->FindObject("galice.root");

    if (!file) {
	printf("\n Creating galice.root \n");
	file = new TFile("galice.root");
    } else {
	printf("\n galice.root found in file list");
    }
    file->ls();

// Get AliRun object from file or create it if not on file
    if (!gAlice) {
	gAlice = (AliRun*)(file->Get("gAlice"));
	if (gAlice) printf("AliRun object found on file\n");
	if (!gAlice) {
	    printf("\n create new gAlice object");
	    gAlice = new AliRun("gAlice","Alice test program");
	}
    }

//  Create some histograms

   TH1F *ccharge1 = new TH1F("ccharge1","Cluster Charge distribution"
			   ,100,0.,500.);
   TH1F *pcharge1 = new TH1F("pcharge1","Pad      Charge distribution"
			   ,100,0.,200.);
   TH1F *xresid1  = new TH1F("xresid1","x-Residuals"
			   ,100,-0.5,0.5);
   TH1F *yresid1  = new TH1F("yresid1","y-Residuals"
			   ,100,-0.2,0.2);
   TH1F *npads1   = new TH1F("npads1" ,"Pads per Hit"
			   ,20,-0.5,19.5);
   TH2F *xresys1  = new TH2F("xresys1","x-Residuals systematics"
			   ,50,-2.0,2.0,100,-1.0,1.0);
   TH2F *yresys1  = new TH2F("yresys1","y-Residuals systematics"
			   ,50,-0.4,0.4,100,-0.1,0.1);

   TH1F *ccharge2 = new TH1F("ccharge2","Cluster Charge distribution"
			   ,100,0.,500.);
   TH1F *pcharge2 = new TH1F("pcharge2","Pad      Charge distribution"
			   ,100,0.,200.);
   TH1F *xresid2  = new TH1F("xresid2","x-Residuals"
			   ,100,-0.5,0.5);
   TH1F *yresid2  = new TH1F("yresid2","y-Residuals"
			   ,100,-0.2,0.2);
   TH1F *npads2   = new TH1F("npads2" ,"Pads per Hit"
			   ,20,-0.5,19.5);
   TH2F *xresys2  = new TH2F("xresys2","x-Residuals systematics"
			   ,50,-2.0,2.0,100,-1.0,1.0);
   TH2F *yresys2  = new TH2F("yresys2","y-Residuals systematics"
			   ,50,-0.4,0.4,100,-0.1,0.1);

   AliRICHchamber*  iChamber;
//
//   Loop over events
//
   Int_t Nh=0;
   Int_t Nh1=0;
   for (Int_t nev=0; nev<= evNumber2; nev++) {
       //cout << "nev         " << nev <<endl;
       printf ("Event Number       : %d\n",nev);
       Int_t nparticles = gAlice->GetEvent(nev);
       //cout << "nparticles  " << nparticles <<endl;
       printf ("Number of particles: %d\n",nparticles);
       if (nev < evNumber1) continue;
       if (nparticles <= 0) return;

       AliRICH *RICH  = (AliRICH*) gAlice->GetDetector("RICH");
	   printf("\n track %d %d \n ", nev, RICH);

       TTree *TH = gAlice->TreeH();
       Int_t ntracks = TH->GetEntries();
       Int_t Nc=0;
       Int_t   npad[2];
       Float_t Q[2], xmean[2],ymean[2],xres[2],yres[2], xonpad[2], yonpad[2];

       printf ("Just got it");
//
//   Loop over events
//
       for (Int_t track=0; track<ntracks;track++) {

	   gAlice->ResetHits();
	   Int_t nbytes += TH->GetEvent(track);
	   if (RICH)  {
	       for(AliRICHhit* mHit=(AliRICHhit*)RICH->FirstHit(-1);
		   mHit;
		   mHit=(AliRICHhit*)RICH->NextHit())
	       {
		   Int_t   nch   = mHit->fChamber;  // chamber number
		   Float_t x     = mHit->fX;        // x-pos of hit
		   Float_t y     = mHit->fY;        // y-pos
//
//
		   iChamber = & RICH->Chamber(nch-1);
		   response=iChamber->GetResponseModel(mip);
//
//
		   if (nch <= 1) {
		       for (Int_t i=0; i<2; i++) {
			   xmean[i]=0;
			   ymean[i]=0;
			   Q[i]=0;
			   npad[i]=0;
		       }
		       for (AliRICHcluster *mPad=(AliRICHcluster*)RICH
				->FirstPad(mHit);
			    mPad;
			    mPad=(AliRICHcluster*)RICH->NextPad()
			   )
		       {
			   Int_t nseg     = mPad->fCathode;   // segmentation
			   Int_t nhit     = mPad->fHitNumber; // hit number
			   Int_t qtot     = mPad->fQ;         // charge
			   Int_t ipx      = mPad->fPadX;      // pad number on X
			   Int_t ipy      = mPad->fPadY;      // pad number on Y
			   Int_t iqpad    = mPad->fQpad;      // charge per pad
			   Int_t secpad   = mPad->fRSec;      // r-pos of pad
//
//
			   segmentation=iChamber->GetSegmentationModel(nseg);
			   Int_t ind=nseg-1;
//		       printf("Pad of hit, padx, pady, iqpad, ncha %d %d %d %d %d %d\n",
//			      nhit, ipx, ipy, iqpad, nseg, ind);

			   if (nseg == 1) {
			       pcharge1->Fill(iqpad,(float) 1);
			       ccharge1->Fill(qtot ,(float) 1);
			   }  else {
			       pcharge2->Fill(iqpad,(float) 1);
			       ccharge2->Fill(qtot ,(float) 1);
			   }
// Calculate centre of gravity
//
			   if (iqpad == 0 && ind==0) {
			       printf("\n attention iqpad 0");
			   }

			   if (iqpad >  0 && secpad==1) {
			       Float_t xc,yc;
			       npad[ind]++;
			       segmentation->GetPadCxy(ipx,ipy,xc,yc);
//			       printf("\n pad %d %d   ", ipx, ipy);
//			       printf("\n pos %f %f   ",  xc,  yc);
			       xmean[ind]+=Float_t(iqpad*xc);
			       ymean[ind]+=Float_t(iqpad*yc);
			       Q[ind]+=iqpad;
			   }

		       } //pad hit loop
		       for (Int_t i=0; i<2; i++) {
			   segmentation = iChamber->GetSegmentationModel(i+1);

//			   if (npad[i] ==0) {
//			       printf("\n #pads=0 %f %f",x,y);
//			   }

			   if (Q[i] >0) {
			       xmean[i] = xmean[i]/Q[i];
			       xres[i]  = xmean[i]-x;
			       ymean[i] = ymean[i]/Q[i];
			       yres[i]  = ymean[i]-y;
//			       printf("\n xmean, ymean %f %f",xmean[i],ymean[i]);
//			       printf("\n xhit, yhit %f %f",x,y);

// Systematic Error
//
			       Int_t icx, icy;
			       segmentation->
				   GetPadIxy(x,y,icx,icy);
			       segmentation->
				   GetPadCxy(icx,icy,xonpad[i],yonpad[i]);
			       xonpad[i]-=x;
			       yonpad[i]-=y;
			   }
		       } // plane loop
		       xresid1->Fill(xres[0],(float) 1);
		       yresid1->Fill(yres[0],(float) 1);
		       npads1->Fill(npad[0],(float) 1);
		       if (npad[0] >=2 && Q[0] > 6 ) {
			   xresys1->Fill(xonpad[0],xres[0],(float) 1);
			   yresys1->Fill(yonpad[0],yres[0],(float) 1);
		       }

		       xresid2->Fill(xres[1],(float) 1);
		       yresid2->Fill(yres[1],(float) 1);
		       npads2->Fill(npad[1],(float) 1);
		       if (npad[1] >=2 && Q[1] > 6) {
			   xresys2->Fill(xonpad[1],xres[1],(float) 1);
			   yresys2->Fill(yonpad[1],yres[1],(float) 1);
		       }
		   } // chamber 1
	       } // hit loop
	   } // if RICH
       } // track loop
   } // event loop

//Create a canvas, set the view range, show histograms
   TCanvas *c1 = new TCanvas("c1","Charge and Residuals",400,10,600,700);
   pad11 = new TPad("pad11"," ",0.01,0.51,0.49,0.99);
   pad12 = new TPad("pad12"," ",0.51,0.51,0.99,0.99);
   pad13 = new TPad("pad13"," ",0.01,0.01,0.49,0.49);
   pad14 = new TPad("pad14"," ",0.51,0.01,0.99,0.49);
   pad11->SetFillColor(11);
   pad12->SetFillColor(11);
   pad13->SetFillColor(11);
   pad14->SetFillColor(11);
   pad11->Draw();
   pad12->Draw();
   pad13->Draw();
   pad14->Draw();

   pad11->cd();
   ccharge1->SetFillColor(42);
   ccharge1->SetXTitle("ADC units");
   ccharge1->Draw();

   pad12->cd();
   pcharge1->SetFillColor(42);
   pcharge1->SetXTitle("ADC units");
   pcharge1->Draw();

   pad13->cd();
   xresid1->SetFillColor(42);
   xresid1->Draw();

   pad14->cd();
   yresid1->SetFillColor(42);
   yresid1->Draw();

   TCanvas *c2 = new TCanvas("c2","Cluster Size",400,10,600,700);
   pad21 = new TPad("pad21"," ",0.01,0.51,0.49,0.99);
   pad22 = new TPad("pad22"," ",0.51,0.51,0.99,0.99);
   pad23 = new TPad("pad23"," ",0.01,0.01,0.49,0.49);
   pad24 = new TPad("pad24"," ",0.51,0.01,0.99,0.49);
   pad21->SetFillColor(11);
   pad22->SetFillColor(11);
   pad23->SetFillColor(11);
   pad24->SetFillColor(11);
   pad21->Draw();
   pad22->Draw();
   pad23->Draw();
   pad24->Draw();

   pad21->cd();
   npads1->SetFillColor(42);
   npads1->SetXTitle("Cluster Size");
   npads1->Draw();

   pad23->cd();
   xresys1->SetXTitle("x on pad");
   xresys1->SetYTitle("x-xcog");
   xresys1->Draw();

   pad24->cd();
   yresys1->SetXTitle("y on pad");
   yresys1->SetYTitle("y-ycog");
   yresys1->Draw();

   TCanvas *c3 = new TCanvas("c3","Charge and Residuals",400,10,600,700);
   pad31 = new TPad("pad31"," ",0.01,0.51,0.49,0.99);
   pad32 = new TPad("pad32"," ",0.51,0.51,0.99,0.99);
   pad33 = new TPad("pad33"," ",0.01,0.01,0.49,0.49);
   pad34 = new TPad("pad34"," ",0.51,0.01,0.99,0.49);
   pad31->SetFillColor(11);
   pad32->SetFillColor(11);
   pad33->SetFillColor(11);
   pad34->SetFillColor(11);
   pad31->Draw();
   pad32->Draw();
   pad33->Draw();
   pad34->Draw();

   pad31->cd();
   ccharge2->SetFillColor(42);
   ccharge2->SetXTitle("ADC units");
   ccharge2->Draw();

   pad32->cd();
   pcharge2->SetFillColor(42);
   pcharge2->SetXTitle("ADC units");
   pcharge2->Draw();

   pad33->cd();
   xresid2->SetFillColor(42);
   xresid2->Draw();

   pad34->cd();
   yresid2->SetFillColor(42);
   yresid2->Draw();

   TCanvas *c4 = new TCanvas("c4","Cluster Size",400,10,600,700);
   pad41 = new TPad("pad41"," ",0.01,0.51,0.49,0.99);
   pad42 = new TPad("pad42"," ",0.51,0.51,0.99,0.99);
   pad43 = new TPad("pad43"," ",0.01,0.01,0.49,0.49);
   pad44 = new TPad("pad44"," ",0.51,0.01,0.99,0.49);
   pad41->SetFillColor(11);
   pad42->SetFillColor(11);
   pad43->SetFillColor(11);
   pad44->SetFillColor(11);
   pad41->Draw();
   pad42->Draw();
   pad43->Draw();
   pad44->Draw();

   pad41->cd();
   npads2->SetFillColor(42);
   npads2->SetXTitle("Cluster Size");
   npads2->Draw();

   pad43->cd();
   xresys2->SetXTitle("x on pad");
   xresys2->SetYTitle("x-xcog");
   xresys2->Draw();

   pad44->cd();
   yresys2->SetXTitle("y on pad");
   yresys2->SetYTitle("y-ycog");
   yresys2->Draw();

}
<commit_msg>obsolete<commit_after><|endoftext|>