<commit_before>#include <iostream>
#include <chrono>
#include <vector>
#include <numeric>
#include "tensorflow/cc/ops/const_op.h"
#include "tensorflow/cc/ops/array_ops.h"
#include "tensorflow/cc/ops/image_ops.h"
#include "tensorflow/core/platform/init_main.h"
#include "tensorflow/core/platform/logging.h"
#include "tensorflow/core/util/command_line_flags.h"
#include "tensorflow/cc/saved_model/loader.h"
#include "tensorflow/compiler/tf2tensorrt/trt_convert_api.h"
#include "tensorflow/core/framework/tensor.h"
#include "tensorflow/core/platform/types.h"
#include "tensorflow/core/framework/graph.pb.h"
#include "tensorflow/core/graph/default_device.h"
#include "tensorflow/core/public/session.h"

using tensorflow::Status;
using tensorflow::string;
using tensorflow::Tensor;
using tensorflow::Flag;

#define TFTRT_ENSURE_OK(x)                                                     \
  do {                                                                         \
    Status s = x;                                                              \
    if (!s.ok()) {                                                             \
      std::cerr << __FILE__ << ":" << __LINE__ << " " << s.error_message()     \
                << std::endl;                                                  \
      return 1;                                                                \
    }                                                                          \
  } while (0)

// Get the name of the GPU.
const string getDeviceName(std::unique_ptr<tensorflow::Session>& session) {
    string device_name = "";
    std::vector<tensorflow::DeviceAttributes> devices;
    Status status = session->ListDevices(&devices);
    if (!status.ok()) { return device_name; }
    for (const auto& d : devices) {
        if (d.device_type() == "GPU" || d.device_type() == "gpu") {
            device_name = d.name();
        }
    }
    return device_name;
}

// Move from the host to the device with `device_name`.
Status moveToDevice(const string& device_name,
                    Tensor& tensor_host,
                    Tensor* tensor_device) {
    // Create identity graph
    tensorflow::Scope root = tensorflow::Scope::NewRootScope();
    auto x = tensorflow::ops::Placeholder(root, tensor_host.dtype());
    auto y = tensorflow::ops::Identity(root, x);

    tensorflow::GraphDef graphDef;
    TF_RETURN_IF_ERROR(root.ToGraphDef(&graphDef));

    // Create session with identity graph
    std::unique_ptr<tensorflow::Session> session(
        tensorflow::NewSession(tensorflow::SessionOptions())
    );
    TF_RETURN_IF_ERROR(session->Create(graphDef));

    // Configure to return output on device
    tensorflow::Session::CallableHandle handle;
    tensorflow::CallableOptions opts;
    opts.add_feed("Placeholder:0");
    opts.add_fetch("Identity:0");
    opts.mutable_fetch_devices()->insert({"Identity:0", device_name});
    opts.set_fetch_skip_sync(true);
    TF_RETURN_IF_ERROR(session->MakeCallable(opts, &handle));

    // Execute graph
    std::vector<Tensor> tensors_device;
    Status status = session->RunCallable(handle, {tensor_host}, &tensors_device, nullptr);
    *tensor_device = tensors_device.front();
    session->ReleaseCallable(handle);
    return status;
}

// Returns info for nodes listed in the signature definition.
std::vector<tensorflow::TensorInfo> getNodeInfo(
    const google::protobuf::Map<std::string, tensorflow::TensorInfo>& signature) {
  std::vector<tensorflow::TensorInfo> info;
  for (const auto& item : signature) {
    info.push_back(item.second);
  }
  return info;
}

// Load the `SavedModel` located at `model_dir`.
Status loadModel(const std::string& model_dir,
                 tensorflow::SavedModelBundle* bundle,
                 std::vector<tensorflow::TensorInfo>* input_info,
                 std::vector<tensorflow::TensorInfo>* output_info) {
    tensorflow::RunOptions run_options;
    tensorflow::SessionOptions sess_options;
    sess_options.config.mutable_gpu_options()->force_gpu_compatible();
    TF_RETURN_IF_ERROR(tensorflow::LoadSavedModel(sess_options, run_options, model_dir, {"serve"}, bundle));

    // Get input and output names
    auto signature_map = bundle->GetSignatures();
    const tensorflow::SignatureDef& signature = signature_map["serving_default"];
    *input_info = getNodeInfo(signature.inputs());
    *output_info = getNodeInfo(signature.outputs());

    return Status::OK();
}

// Create arbitrary inputs matching `input_info` and load them on the device.
Status setupInputs(const string& device_name,
                   int32_t batch_size,
                   std::vector<tensorflow::TensorInfo>& input_info,
                   std::vector<Tensor>* inputs) {
    std::vector<Tensor> inputs_device;
    for (const auto& info : input_info) {
        auto shape = info.tensor_shape();
        shape.mutable_dim(0)->set_size(batch_size);
        Tensor input_host(info.dtype(), shape);
        Tensor input_device;
        TF_RETURN_IF_ERROR(moveToDevice(device_name, input_host, &input_device));
        inputs_device.push_back(input_device);
    }
    *inputs = inputs_device;
    return Status::OK();
}

// Configure a `CallableHandle` that feeds from and fetches to a device.
Status setupCallable(std::unique_ptr<tensorflow::Session>& session,
                     std::vector<tensorflow::TensorInfo>& input_info,
                     std::vector<tensorflow::TensorInfo>& output_info,
                     const string& device_name,
                     tensorflow::Session::CallableHandle* handle) {
    tensorflow::CallableOptions opts;
    for (const auto& info : input_info) {
        const string& name = info.name();
        opts.add_feed(name);
        opts.mutable_feed_devices()->insert({name, device_name});
    }
    for (const auto& info : output_info) {
        const string& name = info.name();
        opts.add_fetch(name);
        opts.mutable_fetch_devices()->insert({name, device_name});
    }
    opts.set_fetch_skip_sync(true);
    return session->MakeCallable(opts, handle);
}

int main(int argc, char* argv[]) {
    // Parse arguments
    string model_path = "/path/to/model/";
    int32_t batch_size = 64;
    int32_t warmup_iters = 50;
    int32_t eval_iters = 1000;
    std::vector<Flag> flag_list = {
        Flag("model_path", &model_path, "graph to be executed"),
        Flag("batch_size", &batch_size, "batch size to use for inference"),
        Flag("warmup_iters", &warmup_iters, "number of warmup iterations to run"),
        Flag("eval_iters", &eval_iters, "number of timed iterations to run"),
    };
    string usage = tensorflow::Flags::Usage(argv[0], flag_list);
    const bool parse_result = tensorflow::Flags::Parse(&argc, argv, flag_list);
    if (!parse_result) {
        LOG(ERROR) << usage;
        return -1;
    }

    // We need to call this to set up global state for TensorFlow.
    tensorflow::port::InitMain(argv[0], &argc, &argv);
    if (argc > 1) {
        LOG(ERROR) << "Unknown argument " << argv[1] << "\n" << usage;
        return -1;
    }

    // Setup session
    tensorflow::SavedModelBundle bundle;
    std::vector<tensorflow::TensorInfo> input_info;
    std::vector<tensorflow::TensorInfo> output_info;
    TFTRT_ENSURE_OK(loadModel(model_path, &bundle, &input_info, &output_info));

    // Create inputs and move to device
    const string device_name = getDeviceName(bundle.session);
    std::vector<Tensor> inputs_device;
    TFTRT_ENSURE_OK(setupInputs(device_name, batch_size, input_info, &inputs_device));

    // Configure to feed and fetch from device
    tensorflow::Session::CallableHandle handle;
    TFTRT_ENSURE_OK(setupCallable(bundle.session, input_info, output_info, device_name, &handle));

    // Run benchmarking
    std::vector<Tensor> outputs;
    std::vector<double> infer_time;
    std::chrono::steady_clock::time_point eval_start_time;
    std::chrono::steady_clock::time_point start_time;
    std::chrono::steady_clock::time_point end_time;
    for (int i = 0; i < warmup_iters + eval_iters; i++) {
        if (i == warmup_iters) {
            LOG(INFO) << "Warmup done";
            eval_start_time = std::chrono::steady_clock::now();
        }

        start_time = std::chrono::steady_clock::now();
        Status status = bundle.session->RunCallable(handle, inputs_device, &outputs, nullptr);
        end_time = std::chrono::steady_clock::now();

        TFTRT_ENSURE_OK(status);
        double duration = (end_time - start_time).count() / 1e6;
        infer_time.push_back(duration);
    }
    TFTRT_ENSURE_OK(bundle.session->ReleaseCallable(handle));

    // Print results
    std::sort(infer_time.begin() + warmup_iters, infer_time.end());
    double total_compute_time = std::accumulate(infer_time.begin() + warmup_iters, infer_time.end(), 0.0);
    double total_wall_time = (end_time - eval_start_time).count() / 1e6;
    int32_t m = eval_iters / 2;
    LOG(INFO) << "Total wall time (s): " << total_wall_time / 1e3;
    LOG(INFO) << "Total GPU compute time (s): " << total_compute_time / 1e3;
    LOG(INFO) << "Mean GPU compute time (ms): " << total_compute_time / eval_iters;
    LOG(INFO) << "Median GPU compute time (ms): " << (eval_iters % 2 ? infer_time[m]
                                                                     : (infer_time[m - 1] + infer_time[m]) / 2);
    LOG(INFO) << "Throughput (ims/s): " << 1e3 * eval_iters * batch_size / total_compute_time;
    LOG(INFO) << "First inference latency (ms): " << infer_time.front();

    return 0;
}<commit_msg>Address PR comments<commit_after>#include <iostream>
#include <chrono>
#include <vector>
#include <numeric>
#include "tensorflow/cc/ops/const_op.h"
#include "tensorflow/cc/ops/array_ops.h"
#include "tensorflow/cc/ops/image_ops.h"
#include "tensorflow/core/platform/init_main.h"
#include "tensorflow/core/platform/logging.h"
#include "tensorflow/core/util/command_line_flags.h"
#include "tensorflow/cc/saved_model/loader.h"
#include "tensorflow/compiler/tf2tensorrt/trt_convert_api.h"
#include "tensorflow/core/framework/tensor.h"
#include "tensorflow/core/platform/types.h"
#include "tensorflow/core/framework/graph.pb.h"
#include "tensorflow/core/graph/default_device.h"
#include "tensorflow/core/public/session.h"

using tensorflow::Status;
using tensorflow::string;
using tensorflow::Tensor;
using tensorflow::Flag;

#define TFTRT_ENSURE_OK(x)                                                     \
  do {                                                                         \
    Status s = x;                                                              \
    if (!s.ok()) {                                                             \
      std::cerr << __FILE__ << ":" << __LINE__ << " " << s.error_message()     \
                << std::endl;                                                  \
      return 1;                                                                \
    }                                                                          \
  } while (0)

// Get the name of the GPU.
const string getDeviceName(std::unique_ptr<tensorflow::Session>& session) {
    string device_name = "";
    std::vector<tensorflow::DeviceAttributes> devices;
    Status status = session->ListDevices(&devices);
    if (!status.ok()) { return device_name; }
    for (const auto& d : devices) {
        if (d.device_type() == "GPU" || d.device_type() == "gpu") {
            device_name = d.name();
        }
    }
    return device_name;
}

// Move from the host to the device with `device_name`.
Status moveToDevice(const string& device_name,
                    Tensor& tensor_host,
                    Tensor* tensor_device) {
    // Create identity graph
    tensorflow::Scope root = tensorflow::Scope::NewRootScope();
    auto x = tensorflow::ops::Placeholder(root, tensor_host.dtype());
    auto y = tensorflow::ops::Identity(root, x);

    tensorflow::GraphDef graphDef;
    TF_RETURN_IF_ERROR(root.ToGraphDef(&graphDef));

    // Create session with identity graph
    std::unique_ptr<tensorflow::Session> session(
        tensorflow::NewSession(tensorflow::SessionOptions())
    );
    TF_RETURN_IF_ERROR(session->Create(graphDef));

    // Configure to return output on device
    tensorflow::Session::CallableHandle handle;
    tensorflow::CallableOptions opts;
    opts.add_feed("Placeholder:0");
    opts.add_fetch("Identity:0");
    opts.mutable_fetch_devices()->insert({"Identity:0", device_name});
    opts.set_fetch_skip_sync(true);
    TF_RETURN_IF_ERROR(session->MakeCallable(opts, &handle));

    // Execute graph
    std::vector<Tensor> tensors_device;
    Status status = session->RunCallable(handle, {tensor_host}, &tensors_device, nullptr);
    *tensor_device = tensors_device.front();
    session->ReleaseCallable(handle);
    return status;
}

// Returns info for nodes listed in the signature definition.
std::vector<tensorflow::TensorInfo> getNodeInfo(
    const google::protobuf::Map<string, tensorflow::TensorInfo>& signature) {
  std::vector<tensorflow::TensorInfo> info;
  for (const auto& item : signature) {
    info.push_back(item.second);
  }
  return info;
}

// Load the `SavedModel` located at `model_dir`.
Status loadModel(const string& model_dir,
                 const string& signature_key,
                 tensorflow::SavedModelBundle* bundle,
                 std::vector<tensorflow::TensorInfo>* input_info,
                 std::vector<tensorflow::TensorInfo>* output_info) {
    tensorflow::RunOptions run_options;
    tensorflow::SessionOptions sess_options;
    sess_options.config.mutable_gpu_options()->force_gpu_compatible();
    TF_RETURN_IF_ERROR(tensorflow::LoadSavedModel(sess_options, run_options, model_dir, {"serve"}, bundle));

    // Get input and output names
    auto signature_map = bundle->GetSignatures();
    const tensorflow::SignatureDef& signature = signature_map[signature_key];
    *input_info = getNodeInfo(signature.inputs());
    *output_info = getNodeInfo(signature.outputs());

    return Status::OK();
}

// Create arbitrary inputs matching `input_info` and load them on the device.
Status setupInputs(const string& device_name,
                   int32_t batch_size,
                   std::vector<tensorflow::TensorInfo>& input_info,
                   std::vector<Tensor>* inputs) {
    std::vector<Tensor> inputs_device;
    for (const auto& info : input_info) {
        auto shape = info.tensor_shape();
        shape.mutable_dim(0)->set_size(batch_size);
        Tensor input_host(info.dtype(), shape);
        Tensor input_device;
        TF_RETURN_IF_ERROR(moveToDevice(device_name, input_host, &input_device));
        inputs_device.push_back(input_device);
    }
    *inputs = inputs_device;
    return Status::OK();
}

// Configure a `CallableHandle` that feeds from and fetches to a device.
Status setupCallable(std::unique_ptr<tensorflow::Session>& session,
                     std::vector<tensorflow::TensorInfo>& input_info,
                     std::vector<tensorflow::TensorInfo>& output_info,
                     const string& device_name,
                     tensorflow::Session::CallableHandle* handle) {
    tensorflow::CallableOptions opts;
    for (const auto& info : input_info) {
        const string& name = info.name();
        opts.add_feed(name);
        opts.mutable_feed_devices()->insert({name, device_name});
    }
    for (const auto& info : output_info) {
        const string& name = info.name();
        opts.add_fetch(name);
        opts.mutable_fetch_devices()->insert({name, device_name});
    }
    opts.set_fetch_skip_sync(true);
    return session->MakeCallable(opts, handle);
}

int main(int argc, char* argv[]) {
    // Parse arguments
    string model_path = "/path/to/model/";
    string signature_key = "serving_default";
    int32_t batch_size = 64;
    int32_t warmup_iters = 50;
    int32_t eval_iters = 1000;
    std::vector<Flag> flag_list = {
        Flag("model_path", &model_path, "graph to be executed"),
        Flag("signature_key", &signature_key, "the serving signature to use"),
        Flag("batch_size", &batch_size, "batch size to use for inference"),
        Flag("warmup_iters", &warmup_iters, "number of warmup iterations to run"),
        Flag("eval_iters", &eval_iters, "number of timed iterations to run"),
    };
    string usage = tensorflow::Flags::Usage(argv[0], flag_list);
    const bool parse_result = tensorflow::Flags::Parse(&argc, argv, flag_list);
    if (!parse_result) {
        LOG(ERROR) << usage;
        return -1;
    }

    // We need to call this to set up global state for TensorFlow.
    tensorflow::port::InitMain(argv[0], &argc, &argv);
    if (argc > 1) {
        LOG(ERROR) << "Unknown argument " << argv[1] << "\n" << usage;
        return -1;
    }

    // Setup session
    tensorflow::SavedModelBundle bundle;
    std::vector<tensorflow::TensorInfo> input_info;
    std::vector<tensorflow::TensorInfo> output_info;
    TFTRT_ENSURE_OK(loadModel(model_path, signature_key, &bundle, &input_info, &output_info));

    // Create inputs and move to device
    const string device_name = getDeviceName(bundle.session);
    std::vector<Tensor> inputs_device;
    TFTRT_ENSURE_OK(setupInputs(device_name, batch_size, input_info, &inputs_device));

    // Configure to feed and fetch from device
    tensorflow::Session::CallableHandle handle;
    TFTRT_ENSURE_OK(setupCallable(bundle.session, input_info, output_info, device_name, &handle));

    // Run benchmarking
    std::vector<Tensor> outputs;
    std::vector<double> infer_time;
    std::chrono::steady_clock::time_point eval_start_time;
    std::chrono::steady_clock::time_point start_time;
    std::chrono::steady_clock::time_point end_time;
    for (int i = 0; i < warmup_iters + eval_iters; i++) {
        if (i == warmup_iters) {
            LOG(INFO) << "Warmup done";
            eval_start_time = std::chrono::steady_clock::now();
        }

        start_time = std::chrono::steady_clock::now();
        Status status = bundle.session->RunCallable(handle, inputs_device, &outputs, nullptr);
        end_time = std::chrono::steady_clock::now();

        TFTRT_ENSURE_OK(status);
        double duration = (end_time - start_time).count() / 1e6;
        infer_time.push_back(duration);
    }
    TFTRT_ENSURE_OK(bundle.session->ReleaseCallable(handle));

    // Print results
    std::sort(infer_time.begin() + warmup_iters, infer_time.end());
    double total_compute_time = std::accumulate(infer_time.begin() + warmup_iters, infer_time.end(), 0.0);
    double total_wall_time = (end_time - eval_start_time).count() / 1e6;
    int32_t m = warmup_iters + eval_iters / 2;
    LOG(INFO) << "Total wall time (s): " << total_wall_time / 1e3;
    LOG(INFO) << "Total GPU compute time (s): " << total_compute_time / 1e3;
    LOG(INFO) << "Mean GPU compute time (ms): " << total_compute_time / eval_iters;
    LOG(INFO) << "Median GPU compute time (ms): " << (eval_iters % 2 ? infer_time[m]
                                                                     : (infer_time[m - 1] + infer_time[m]) / 2);
    // Note: Throughput using GPU inference time, rather than wall time
    LOG(INFO) << "Throughput (samples/s): " << 1e3 * eval_iters * batch_size / total_compute_time;
    LOG(INFO) << "First inference latency (ms): " << infer_time.front();

    return 0;
}<|endoftext|>


<commit_before>#ifndef RESULT_ALL_HPP_
#define RESULT_ALL_HPP_

#include "result_benchmark.hpp"
#include "traits.hpp"
#include "types.hpp"

#include <sstream>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <vector>
#include <string>
#include <algorithm>

namespace gearshifft {

  template<int T_NumberRuns,  // runs per benchmark including warmup
           int T_NumberWarmups,
           int T_NumberValues // recorded values per run
           >
  class ResultAll {
  public:
    using ResultBenchmarkT = ResultBenchmark<T_NumberRuns, T_NumberValues>;

    void add(const ResultBenchmarkT& result) {
      results_.push_back(result);
    }

    /*
     * sort order:  fftkind -> dimkind -> dim -> nx*ny*nz
     */
    void sort() {
      std::stable_sort(
        results_.begin( ), results_.end( ),
        [ ]( const ResultBenchmarkT& lhs, const ResultBenchmarkT& rhs )
        {
          if(lhs.getPrecision()==rhs.getPrecision())
            if(lhs.isInplace()==rhs.isInplace())
              if(lhs.isComplex()==rhs.isComplex())
                return lhs.getDimKind()<rhs.getDimKind() ||
                                        lhs.getDimKind()==rhs.getDimKind() &&
                                        (
                                          lhs.getDim()<rhs.getDim() ||
                                          lhs.getDim()==rhs.getDim() &&
                                          lhs.getExtentsTotal()<rhs.getExtentsTotal()
                                          );
              else
                return lhs.isComplex();
            else
              return lhs.isInplace();
          else
            return false;
        });
    }

    void print(std::ostream& stream,
               const std::string& apptitle,
               const std::string& dev_infos,
               double timerContextCreate,
               double timerContextDestroy) {
      std::stringstream ss;
      ss << "; " << dev_infos << "\n"
         << "; \"Time_ContextCreate [ms]\", " << timerContextCreate << "\n"
         << "; \"Time_ContextDestroy [ms]\", " << timerContextDestroy  << "\n";
      ss << apptitle
         << ", RunsPerBenchmark="<<T_NumberRuns
         << "\n";

      for(auto& result : results_) {
        int nruns = T_NumberRuns;
        std::string inplace = result.isInplace() ? "Inplace" : "Outplace";
        std::string complex = result.isComplex() ? "Complex" : "Real";

        ss << std::setfill('-') << std::setw(70) <<"-"<< "\n";
        ss << inplace
           << ", "<<complex
           << ", "<<result.getPrecision()
           << ", Dim="<<result.getDim()
           << ", Kind="<<result.getDimKindStr()<<" ("<<result.getDimKind()<<")"
           << ", Ext="<<result.getExtents()
           << "\n";
        if(result.hasError()) {
          ss << " Error at run="<<result.getErrorRun()
             << ": "<<result.getError()
             << "\n";
          nruns = result.getErrorRun()+1;
        }
        ss << std::setfill('-') << std::setw(70) <<"-"<< "\n";
        ss << std::setfill(' ');
        double sum;
        for(int ival=0; ival<T_NumberValues; ++ival) {
          sum = 0.0;
          for(int run=T_NumberWarmups; run<nruns; ++run) {
            result.setRun(run);
            sum += result.getValue(ival);
          }
          ss << std::setw(28)
            << static_cast<RecordType>(ival)
             << ": " << std::setw(16) << sum/nruns
             << " [avg]"
             << "\n";
        }
      }
      stream << ss.str() << std::endl; // "\n" with flush
    }

    /**
     * Store results in csv file.
     * If verbosity flag is set, then std::cout receives result view.
     */
    void saveCSV(const std::string& fname,
                 const std::string& apptitle,
                 const std::string& meta_information,
                 double timerContextCreate,
                 double timerContextDestroy) {
      std::ofstream fs;
      const char sep=',';

      fs.open(fname, std::ofstream::out);
      fs.precision(11);
      fs << "; " << meta_information <<"\n"
         << "; \"Time_ContextCreate [ms]\", " << timerContextCreate << "\n"
         << "; \"Time_ContextDestroy [ms]\", " << timerContextDestroy  << "\n";
      // header
      fs << "\"library\",\"inplace\",\"complex\",\"precision\",\"dim\",\"kind\""
         << ",\"nx\",\"ny\",\"nz\",\"run\",\"id\",\"success\"";
      for(auto ival=0; ival<T_NumberValues; ++ival) {
        fs << sep << '"' << static_cast<RecordType>(ival) << '"';
      }
      fs << "\n";

      // data
      for(auto& result : results_) {
        std::string inplace = result.isInplace() ? "Inplace" : "Outplace";
        std::string complex = result.isComplex() ? "Complex" : "Real";
        for(auto run=0; run<T_NumberRuns; ++run) {
          result.setRun(run);
          fs << "\"" << apptitle << "\"" << sep
             << "\"" << inplace  << "\"" << sep
             << "\"" << complex  << "\"" << sep
             << "\"" << result.getPrecision() << "\"" << sep
             << result.getDim() << sep
             << "\"" << result.getDimKindStr() << "\"" << sep
             << result.getExtents()[0] << sep
             << result.getExtents()[1] << sep
             << result.getExtents()[2] << sep
             << run << sep
             << result.getID();
          // was run successfull?
          if(result.hasError() && result.getErrorRun()<=run) {
            if(result.getErrorRun()==run)
              fs << sep << "\"" <<result.getError() << "\"";
            else
              fs << sep << "\"Skipped\""; // subsequent runs did not run
          } else {
            if(run<T_NumberWarmups)
              fs << sep << "\"" << "Warmup" << "\"";
            else
              fs << sep << "\"" << "Success" << "\"";
          }
          // measured time and size values
          for(auto ival=0; ival<T_NumberValues; ++ival) {
            fs << sep << result.getValue(ival);
          }
          fs << "\n";
        } // run
      } // result
      fs.close();
    } // write

  private:
    std::vector< ResultBenchmarkT > results_;

  };

}
#endif
<commit_msg>fixes wrong avg computation in verbose mode.<commit_after>#ifndef RESULT_ALL_HPP_
#define RESULT_ALL_HPP_

#include "result_benchmark.hpp"
#include "traits.hpp"
#include "types.hpp"

#include <sstream>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <vector>
#include <string>
#include <algorithm>

namespace gearshifft {

  template<int T_NumberRuns,  // runs per benchmark including warmup
           int T_NumberWarmups,
           int T_NumberValues // recorded values per run
           >
  class ResultAll {
  public:
    using ResultBenchmarkT = ResultBenchmark<T_NumberRuns, T_NumberValues>;

    void add(const ResultBenchmarkT& result) {
      results_.push_back(result);
    }

    /*
     * sort order:  fftkind -> dimkind -> dim -> nx*ny*nz
     */
    void sort() {
      std::stable_sort(
        results_.begin( ), results_.end( ),
        [ ]( const ResultBenchmarkT& lhs, const ResultBenchmarkT& rhs )
        {
          if(lhs.getPrecision()==rhs.getPrecision())
            if(lhs.isInplace()==rhs.isInplace())
              if(lhs.isComplex()==rhs.isComplex())
                return lhs.getDimKind()<rhs.getDimKind() ||
                                        lhs.getDimKind()==rhs.getDimKind() &&
                                        (
                                          lhs.getDim()<rhs.getDim() ||
                                          lhs.getDim()==rhs.getDim() &&
                                          lhs.getExtentsTotal()<rhs.getExtentsTotal()
                                          );
              else
                return lhs.isComplex();
            else
              return lhs.isInplace();
          else
            return false;
        });
    }

    void print(std::ostream& stream,
               const std::string& apptitle,
               const std::string& dev_infos,
               double timerContextCreate,
               double timerContextDestroy) {
      std::stringstream ss;
      ss << "; " << dev_infos << "\n"
         << "; \"Time_ContextCreate [ms]\", " << timerContextCreate << "\n"
         << "; \"Time_ContextDestroy [ms]\", " << timerContextDestroy  << "\n";
      ss << apptitle
         << ", RunsPerBenchmark="<<T_NumberRuns
         << "\n";

      for(auto& result : results_) {
        int nruns = T_NumberRuns;
        std::string inplace = result.isInplace() ? "Inplace" : "Outplace";
        std::string complex = result.isComplex() ? "Complex" : "Real";

        ss << std::setfill('-') << std::setw(70) <<"-"<< "\n";
        ss << inplace
           << ", "<<complex
           << ", "<<result.getPrecision()
           << ", Dim="<<result.getDim()
           << ", Kind="<<result.getDimKindStr()<<" ("<<result.getDimKind()<<")"
           << ", Ext="<<result.getExtents()
           << "\n";
        if(result.hasError()) {
          ss << " Error at run="<<result.getErrorRun()
             << ": "<<result.getError()
             << "\n";
          nruns = result.getErrorRun()+1;
        }
        ss << std::setfill('-') << std::setw(70) <<"-"<< "\n";
        ss << std::setfill(' ');
        double sum;
        for(int ival=0; ival<T_NumberValues; ++ival) {
          sum = 0.0;
          for(int run=T_NumberWarmups; run<nruns; ++run) {
            result.setRun(run);
            sum += result.getValue(ival);
          }
          ss << std::setw(28)
            << static_cast<RecordType>(ival)
             << ": " << std::setw(16) << sum/(T_NumberRuns-T_NumberWarmups)
             << " [avg]"
             << "\n";
        }
      }
      stream << ss.str() << std::endl; // "\n" with flush
    }

    /**
     * Store results in csv file.
     * If verbosity flag is set, then std::cout receives result view.
     */
    void saveCSV(const std::string& fname,
                 const std::string& apptitle,
                 const std::string& meta_information,
                 double timerContextCreate,
                 double timerContextDestroy) {
      std::ofstream fs;
      const char sep=',';

      fs.open(fname, std::ofstream::out);
      fs.precision(11);
      fs << "; " << meta_information <<"\n"
         << "; \"Time_ContextCreate [ms]\", " << timerContextCreate << "\n"
         << "; \"Time_ContextDestroy [ms]\", " << timerContextDestroy  << "\n";
      // header
      fs << "\"library\",\"inplace\",\"complex\",\"precision\",\"dim\",\"kind\""
         << ",\"nx\",\"ny\",\"nz\",\"run\",\"id\",\"success\"";
      for(auto ival=0; ival<T_NumberValues; ++ival) {
        fs << sep << '"' << static_cast<RecordType>(ival) << '"';
      }
      fs << "\n";

      // data
      for(auto& result : results_) {
        std::string inplace = result.isInplace() ? "Inplace" : "Outplace";
        std::string complex = result.isComplex() ? "Complex" : "Real";
        for(auto run=0; run<T_NumberRuns; ++run) {
          result.setRun(run);
          fs << "\"" << apptitle << "\"" << sep
             << "\"" << inplace  << "\"" << sep
             << "\"" << complex  << "\"" << sep
             << "\"" << result.getPrecision() << "\"" << sep
             << result.getDim() << sep
             << "\"" << result.getDimKindStr() << "\"" << sep
             << result.getExtents()[0] << sep
             << result.getExtents()[1] << sep
             << result.getExtents()[2] << sep
             << run << sep
             << result.getID();
          // was run successfull?
          if(result.hasError() && result.getErrorRun()<=run) {
            if(result.getErrorRun()==run)
              fs << sep << "\"" <<result.getError() << "\"";
            else
              fs << sep << "\"Skipped\""; // subsequent runs did not run
          } else {
            if(run<T_NumberWarmups)
              fs << sep << "\"" << "Warmup" << "\"";
            else
              fs << sep << "\"" << "Success" << "\"";
          }
          // measured time and size values
          for(auto ival=0; ival<T_NumberValues; ++ival) {
            fs << sep << result.getValue(ival);
          }
          fs << "\n";
        } // run
      } // result
      fs.close();
    } // write

  private:
    std::vector< ResultBenchmarkT > results_;

  };

}
#endif
<|endoftext|>


<commit_before>// SPDX-License-Identifier: MIT
/**
   Copyright (c) 2021 - 2022 Beckhoff Automation GmbH & Co. KG
 */

#include "RouterAccess.h"
#include "Log.h"
#include "wrap_endian.h"
#include <array>
#include <iostream>

namespace bhf
{
namespace ads
{
struct SearchPciBusReq {
    SearchPciBusReq(const uint64_t quad)
        : leVendorID(bhf::ads::htole<uint16_t>(quad >> 48))
        , leDeviceID(bhf::ads::htole<uint16_t>(quad >> 32))
        , leSubVendorID(bhf::ads::htole<uint16_t>(quad >> 16))
        , leSubSystemID(bhf::ads::htole<uint16_t>(quad))
    {}
private:
    const uint16_t leVendorID;
    const uint16_t leDeviceID;
    const uint16_t leSubVendorID;
    const uint16_t leSubSystemID;
};

struct SearchPciSlotResNew {
    static constexpr size_t MAXBASEADDRESSES = 6;
    std::array<uint32_t, MAXBASEADDRESSES> leBaseAddresses;
    uint32_t leSize[MAXBASEADDRESSES];
    uint32_t leBusNumber;
    uint32_t leSlotNumber;
    uint16_t leBoardIrq;
    uint16_t lePciRegViaPorts;
};

struct SearchPciBusResNew {
    static constexpr size_t MAXSLOTRESPONSE = 64;
    uint32_t leFound;
    std::array<SearchPciSlotResNew, MAXSLOTRESPONSE> slot;
    uint32_t nFound() const
    {
        return bhf::ads::letoh(leFound);
    }
};

std::ostream& operator<<(std::ostream& os, const SearchPciSlotResNew& slot)
{
    return os << std::dec << bhf::ads::letoh(slot.leBusNumber) << ':' <<
           bhf::ads::letoh(slot.leSlotNumber) << " @ 0x" <<
           bhf::ads::letoh(slot.leBaseAddresses[0]);
}

RouterAccess::RouterAccess(const std::string& gw, const AmsNetId netid, const uint16_t port)
    : device(gw, netid, port ? port : 1)
{}

bool RouterAccess::PciScan(const uint64_t pci_id, std::ostream& os) const
{
#define ROUTERADSGRP_ACCESS_HARDWARE 0x00000005
#define ROUTERADSOFFS_A_HW_SEARCHPCIBUS 0x00000003

    SearchPciBusResNew res {};
    uint32_t bytesRead;

    const auto req = SearchPciBusReq {pci_id};
    const auto status = device.ReadWriteReqEx2(
        ROUTERADSGRP_ACCESS_HARDWARE,
        ROUTERADSOFFS_A_HW_SEARCHPCIBUS,
        sizeof(res), &res,
        sizeof(req), &req,
        &bytesRead
        );

    if (ADSERR_NOERR != status) {
        LOG_ERROR(__FUNCTION__ << "(): failed with: 0x" << std::hex << status << '\n');
        return false;
    }

    if (res.slot.size() < res.nFound()) {
        LOG_WARN(__FUNCTION__
                 << "(): data seems corrupt. Slot count 0x" << std::hex << res.nFound() << " exceeds maximum 0x" << res.slot.size() <<
                 " -> truncating\n");
    }

    auto limit = std::min<size_t>(res.slot.size(), res.nFound());
    os << "PCI devices found: " << std::dec << limit << '\n';
    for (const auto& slot: res.slot) {
        if (!limit--) {
            break;
        }
        os << slot << '\n';
    }
    return !os.good();
}
}
}
<commit_msg>RouterAccess: please MSVC even more<commit_after>// SPDX-License-Identifier: MIT
/**
   Copyright (c) 2021 - 2022 Beckhoff Automation GmbH & Co. KG
 */

#include "RouterAccess.h"
#include "Log.h"
#include "wrap_endian.h"
#include <array>
#include <iostream>

namespace bhf
{
namespace ads
{
struct SearchPciBusReq {
    SearchPciBusReq(const uint64_t quad)
        : leVendorID(bhf::ads::htole<uint16_t>(static_cast<uint16_t>(quad >> 48)))
        , leDeviceID(bhf::ads::htole<uint16_t>(static_cast<uint16_t>(quad >> 32)))
        , leSubVendorID(bhf::ads::htole<uint16_t>(static_cast<uint16_t>(quad >> 16)))
        , leSubSystemID(bhf::ads::htole<uint16_t>(static_cast<uint16_t>(quad)))
    {}
private:
    const uint16_t leVendorID;
    const uint16_t leDeviceID;
    const uint16_t leSubVendorID;
    const uint16_t leSubSystemID;
};

struct SearchPciSlotResNew {
    static constexpr size_t MAXBASEADDRESSES = 6;
    std::array<uint32_t, MAXBASEADDRESSES> leBaseAddresses;
    uint32_t leSize[MAXBASEADDRESSES];
    uint32_t leBusNumber;
    uint32_t leSlotNumber;
    uint16_t leBoardIrq;
    uint16_t lePciRegViaPorts;
};

struct SearchPciBusResNew {
    static constexpr size_t MAXSLOTRESPONSE = 64;
    uint32_t leFound;
    std::array<SearchPciSlotResNew, MAXSLOTRESPONSE> slot;
    uint32_t nFound() const
    {
        return bhf::ads::letoh(leFound);
    }
};

std::ostream& operator<<(std::ostream& os, const SearchPciSlotResNew& slot)
{
    return os << std::dec << bhf::ads::letoh(slot.leBusNumber) << ':' <<
           bhf::ads::letoh(slot.leSlotNumber) << " @ 0x" <<
           bhf::ads::letoh(slot.leBaseAddresses[0]);
}

RouterAccess::RouterAccess(const std::string& gw, const AmsNetId netid, const uint16_t port)
    : device(gw, netid, port ? port : 1)
{}

bool RouterAccess::PciScan(const uint64_t pci_id, std::ostream& os) const
{
#define ROUTERADSGRP_ACCESS_HARDWARE 0x00000005
#define ROUTERADSOFFS_A_HW_SEARCHPCIBUS 0x00000003

    SearchPciBusResNew res {};
    uint32_t bytesRead;

    const auto req = SearchPciBusReq {pci_id};
    const auto status = device.ReadWriteReqEx2(
        ROUTERADSGRP_ACCESS_HARDWARE,
        ROUTERADSOFFS_A_HW_SEARCHPCIBUS,
        sizeof(res), &res,
        sizeof(req), &req,
        &bytesRead
        );

    if (ADSERR_NOERR != status) {
        LOG_ERROR(__FUNCTION__ << "(): failed with: 0x" << std::hex << status << '\n');
        return false;
    }

    if (res.slot.size() < res.nFound()) {
        LOG_WARN(__FUNCTION__
                 << "(): data seems corrupt. Slot count 0x" << std::hex << res.nFound() << " exceeds maximum 0x" << res.slot.size() <<
                 " -> truncating\n");
    }

    auto limit = std::min<size_t>(res.slot.size(), res.nFound());
    os << "PCI devices found: " << std::dec << limit << '\n';
    for (const auto& slot: res.slot) {
        if (!limit--) {
            break;
        }
        os << slot << '\n';
    }
    return !os.good();
}
}
}
<|endoftext|>


<commit_before>/*
 *  Copyright 2008-2009 NVIDIA Corporation
 *
 *  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.
 */

#pragma once

#include <thrust/iterator/counting_iterator.h>
#include <thrust/iterator/iterator_traits.h>
#include <thrust/detail/numeric_traits.h>
#include <thrust/detail/device/dereference.h>

namespace thrust
{

// forward declaration of counting_iterator
template <typename Incrementable, typename Space, typename Traversal, typename Difference>
  class counting_iterator;

namespace detail
{

template <typename Incrementable, typename Space, typename Traversal, typename Difference>
  struct counting_iterator_base
{
  typedef typename thrust::experimental::detail::ia_dflt_help<
    Space,
    thrust::detail::identity_<thrust::any_space_tag>
  >::type space;

  typedef typename thrust::experimental::detail::ia_dflt_help<
      Traversal,
      thrust::detail::eval_if<
          thrust::detail::is_numeric<Incrementable>::value,
          thrust::detail::identity_<random_access_traversal_tag>,
          thrust::iterator_traversal<Incrementable>
      >
  >::type traversal;

  // XXX this is equivalent to Boost's implementation
  //typedef typename detail::ia_dflt_help<
  //  Difference,
  //  eval_if<
  //    is_numeric<Incrementable>::value,
  //    numeric_difference<Incrementable>,
  //    iterator_difference<Incrementable>
  //  >
  //>::type difference;

  typedef typename thrust::experimental::detail::ia_dflt_help<
    Difference,
    thrust::detail::eval_if<
      thrust::detail::is_numeric<Incrementable>::value,
      thrust::detail::identity_<std::ptrdiff_t>,
      thrust::iterator_difference<Incrementable>
    >
  >::type difference;

  typedef thrust::experimental::iterator_adaptor<
    counting_iterator<Incrementable, Space, Traversal, Difference>, // self
    Incrementable,                                                  // Base
    Incrementable *,                                                // Pointer -- maybe we should make this device_ptr when memory space category is device?
    Incrementable,                                                  // Value
    space,
    traversal,
    Incrementable const &,
    difference
  > type;
}; // end counting_iterator_base

namespace device
{

// specialize dereference_result for counting_iterator
// transform_iterator returns the same reference on the device as on the host
template <typename Incrementable, typename Space, typename Traversal, typename Difference>
  struct dereference_result<
    thrust::counting_iterator<
      Incrementable, Space, Traversal, Difference
    >
  >
{
  typedef typename thrust::iterator_traits< thrust::counting_iterator<Incrementable,Space,Traversal,Difference> >::reference type;
}; // end dereference_result

template<typename Incrementable, typename Space, typename Traversal, typename Difference>
  inline __host__ __device__
    typename dereference_result< thrust::counting_iterator<Incrementable,Space,Traversal,Difference> >::type
      dereference(const thrust::counting_iterator<Incrementable,Space,Traversal,Difference> &iter)
{
  return *iter;
} // end dereference()

template<typename Incrementable, typename Space, typename Traversal, typename Difference, typename IndexType>
  inline __host__ __device__
    typename dereference_result< thrust::counting_iterator<Incrementable,Space,Traversal,Difference> >::type
      dereference(const thrust::counting_iterator<Incrementable,Space,Traversal,Difference> &iter, IndexType n)
{
  return iter[n];
} // end dereference()

} // end device

template<typename Difference, typename Incrementable1, typename Incrementable2>
  struct iterator_distance
{
  __host__ __device__
  static Difference distance(Incrementable1 x, Incrementable2 y)
  {
    return y - x;
  }
};

template<typename Difference, typename Incrementable1, typename Incrementable2>
  struct number_distance
{
  __host__ __device__
  static Difference distance(Incrementable1 x, Incrementable2 y)
  {
    return numeric_distance(x,y);
  }
};

} // end detail

} // end thrust

<commit_msg>When counting_iterator is given device_space_tag for its Space parameter, it needs to convert it to default_backend_space_tag<commit_after>/*
 *  Copyright 2008-2009 NVIDIA Corporation
 *
 *  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.
 */

#pragma once

#include <thrust/iterator/counting_iterator.h>
#include <thrust/iterator/iterator_traits.h>
#include <thrust/detail/numeric_traits.h>
#include <thrust/detail/device/dereference.h>
#include <thrust/iterator/detail/backend_iterator_spaces.h>

namespace thrust
{

// forward declaration of counting_iterator
template <typename Incrementable, typename Space, typename Traversal, typename Difference>
  class counting_iterator;

namespace detail
{

template <typename Incrementable, typename Space, typename Traversal, typename Difference>
  struct counting_iterator_base
{
  typedef typename thrust::detail::eval_if<
    // use any_space_tag if we are given use_default
    thrust::detail::is_same<Space,use_default>::value,
    thrust::detail::identity_<thrust::any_space_tag>,
    thrust::detail::eval_if<
      // use the default backend space if we are given device_space_tag
      thrust::detail::is_same<Space,thrust::device_space_tag>::value,
      thrust::detail::identity_<thrust::detail::default_device_space_tag>,
      thrust::detail::identity_<Space>
    >
  >::type space;

  typedef typename thrust::experimental::detail::ia_dflt_help<
      Traversal,
      thrust::detail::eval_if<
          thrust::detail::is_numeric<Incrementable>::value,
          thrust::detail::identity_<random_access_traversal_tag>,
          thrust::iterator_traversal<Incrementable>
      >
  >::type traversal;

  // XXX this is equivalent to Boost's implementation
  //typedef typename detail::ia_dflt_help<
  //  Difference,
  //  eval_if<
  //    is_numeric<Incrementable>::value,
  //    numeric_difference<Incrementable>,
  //    iterator_difference<Incrementable>
  //  >
  //>::type difference;

  typedef typename thrust::experimental::detail::ia_dflt_help<
    Difference,
    thrust::detail::eval_if<
      thrust::detail::is_numeric<Incrementable>::value,
      thrust::detail::identity_<std::ptrdiff_t>,
      thrust::iterator_difference<Incrementable>
    >
  >::type difference;

  typedef thrust::experimental::iterator_adaptor<
    counting_iterator<Incrementable, Space, Traversal, Difference>, // self
    Incrementable,                                                  // Base
    Incrementable *,                                                // Pointer -- maybe we should make this device_ptr when memory space category is device?
    Incrementable,                                                  // Value
    space,
    traversal,
    Incrementable const &,
    difference
  > type;
}; // end counting_iterator_base

namespace device
{

// specialize dereference_result for counting_iterator
// transform_iterator returns the same reference on the device as on the host
template <typename Incrementable, typename Space, typename Traversal, typename Difference>
  struct dereference_result<
    thrust::counting_iterator<
      Incrementable, Space, Traversal, Difference
    >
  >
{
  typedef typename thrust::iterator_traits< thrust::counting_iterator<Incrementable,Space,Traversal,Difference> >::reference type;
}; // end dereference_result

template<typename Incrementable, typename Space, typename Traversal, typename Difference>
  inline __host__ __device__
    typename dereference_result< thrust::counting_iterator<Incrementable,Space,Traversal,Difference> >::type
      dereference(const thrust::counting_iterator<Incrementable,Space,Traversal,Difference> &iter)
{
  return *iter;
} // end dereference()

template<typename Incrementable, typename Space, typename Traversal, typename Difference, typename IndexType>
  inline __host__ __device__
    typename dereference_result< thrust::counting_iterator<Incrementable,Space,Traversal,Difference> >::type
      dereference(const thrust::counting_iterator<Incrementable,Space,Traversal,Difference> &iter, IndexType n)
{
  return iter[n];
} // end dereference()

} // end device

template<typename Difference, typename Incrementable1, typename Incrementable2>
  struct iterator_distance
{
  __host__ __device__
  static Difference distance(Incrementable1 x, Incrementable2 y)
  {
    return y - x;
  }
};

template<typename Difference, typename Incrementable1, typename Incrementable2>
  struct number_distance
{
  __host__ __device__
  static Difference distance(Incrementable1 x, Incrementable2 y)
  {
    return numeric_distance(x,y);
  }
};

} // end detail

} // end thrust

<|endoftext|>


<commit_before>#include "tidyup_actions/stateCreatorRobotPose.h"
#include <pluginlib/class_list_macros.h>
#include <visualization_msgs/MarkerArray.h>
#include <angles/angles.h>

PLUGINLIB_DECLARE_CLASS(tidyup_actions, state_creator_robot_pose,
        tidyup_actions::StateCreatorRobotPose, continual_planning_executive::StateCreator)

namespace tidyup_actions
{

    StateCreatorRobotPose::StateCreatorRobotPose()
    {
        ros::NodeHandle nhPriv("~");
        nhPriv.param("nav_target_tolerance_xy", _goalToleranceXY, 0.5);
        nhPriv.param("nav_target_tolerance_yaw", _goalToleranceYaw, 0.26);  //15deg

        bool relative;
        nhPriv.param("nav_target_tolerance_relative_to_move_base", relative, false);
        if(relative) {
            // relative mode: 1. get the namespace for base_local_planner
            std::string base_local_planner_ns;
            if(!nhPriv.getParam("nav_base_local_planner_ns", base_local_planner_ns)) {
                // TODO can we estimate this?
                ROS_ERROR("nav_target_tolerance_relative_to_move_base was true, but nav_base_local_planner_ns is not set - falling back to absolute mode.");
            } else { // success: 2. get the xy_goal_tolerance
                double move_base_tol_xy;
                ros::NodeHandle nh;
                if(!nh.getParam(base_local_planner_ns + "/xy_goal_tolerance", move_base_tol_xy)) {
                    ROS_ERROR_STREAM("nav_target_tolerance_relative_to_move_base was true, but "
                            << (base_local_planner_ns + "/xy_goal_tolerance") << " was not set"
                            << " - falling back to absolute mode");
                } else { // 2. add move_base's tolerance to our relative tolerance
                    _goalToleranceXY += move_base_tol_xy;
                }

                double move_base_tol_yaw;
                if(!nh.getParam(base_local_planner_ns + "/yaw_goal_tolerance", move_base_tol_yaw)) {
                    ROS_ERROR_STREAM("nav_target_tolerance_relative_to_move_base was true, but "
                            << (base_local_planner_ns + "/yaw_goal_tolerance") << " was not set"
                            << " - falling back to absolute mode");
                } else { // 2. add move_base's tolerance to our relative tolerance
                    _goalToleranceYaw += move_base_tol_yaw;
                }
            }
        }

        ROS_INFO("Tolerance for accepting nav goals set to %f m, %f deg.",
                _goalToleranceXY, angles::to_degrees(_goalToleranceYaw));

        if(s_PublishLocationsAsMarkers) {
            _markerPub = nhPriv.advertise<visualization_msgs::MarkerArray>("robot_pose_markers", 5, true);
            ROS_INFO("marker topic: %s", _markerPub.getTopic().c_str());
        }
    }

    StateCreatorRobotPose::~StateCreatorRobotPose()
    {
    }

    void StateCreatorRobotPose::initialize(const std::deque<std::string> & arguments)
    {
        ROS_ASSERT(arguments.size() == 4);

        _robotPoseObject = arguments[0];
        _robotPoseType = arguments[1];
        _atPredicate = arguments[2];
        _locationType = arguments[3];

        if(_robotPoseObject == "-")
            _robotPoseObject = "";
        if(_robotPoseType == "-")
            _robotPoseType = "";
        if(_atPredicate == "-")
            _atPredicate = "";
        if(_locationType == "-")
            _locationType = "";
    }

    bool StateCreatorRobotPose::fillState(SymbolicState & state)
    {
        tf::StampedTransform transform;
        try{
            _tf.lookupTransform("/map", "/base_link", ros::Time(0), transform);
        }
        catch (tf::TransformException ex){
            ROS_ERROR("%s",ex.what());
            return false;
        }

        // 1. Real robot location
        if(!_robotPoseObject.empty()) {
            ROS_ASSERT(!_robotPoseType.empty());
            state.addObject(_robotPoseObject, _robotPoseType);
            state.setNumericalFluent("x", _robotPoseObject, transform.getOrigin().x());
            state.setNumericalFluent("y", _robotPoseObject, transform.getOrigin().y());
            state.setNumericalFluent("z", _robotPoseObject, transform.getOrigin().z());
            state.setNumericalFluent("qx", _robotPoseObject, transform.getRotation().x());
            state.setNumericalFluent("qy", _robotPoseObject, transform.getRotation().y());
            state.setNumericalFluent("qz", _robotPoseObject, transform.getRotation().z());
            state.setNumericalFluent("qw", _robotPoseObject, transform.getRotation().w());
            state.setNumericalFluent("timestamp", _robotPoseObject, ros::Time::now().toSec());
            state.addObject("/map", "frameid");
            state.setObjectFluent("frame-id", _robotPoseObject, "/map");
        }

        // 2.b check if we are at any _locations
        pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> targets =
            state.getTypedObjects().equal_range(_locationType);

        vector<string> paramList;
        paramList.push_back("dummy");

        double minDist = HUGE_VAL;
        string nearestTarget = "";

        int atLocations = 0;
        for(SymbolicState::TypedObjectConstIterator it = targets.first; it != targets.second; it++) {
            string target = it->second;
            if(target == _robotPoseObject)  // skip current robot location
                continue;

            // first get xyz, qxyzw from state
            Predicate p;
            paramList[0] = target;
            p.parameters = paramList;

            p.name = "x";
            double posX;
            if(!state.hasNumericalFluent(p, &posX)) {
                ROS_ERROR("%s: target object: %s - no x-location in state.", __func__, target.c_str());
                continue;
            }
            double posY;
            p.name = "y";
            if(!state.hasNumericalFluent(p, &posY)) {
                ROS_ERROR("%s: target object: %s - no y-location in state.", __func__, target.c_str());
                continue;
            }

            double qx;
            p.name = "qx";
            if(!state.hasNumericalFluent(p, &qx)) {
                ROS_ERROR("%s: target object: %s - no qx in state.", __func__, target.c_str());
                continue;
            }
            double qy;
            p.name = "qy";
            if(!state.hasNumericalFluent(p, &qy)) {
                ROS_ERROR("%s: target object: %s - no qy in state.", __func__, target.c_str());
                continue;
            }
            double qz;
            p.name = "qz";
            if(!state.hasNumericalFluent(p, &qz)) {
                ROS_ERROR("%s: target object: %s - no qz in state.", __func__, target.c_str());
                continue;
            }
            double qw;
            p.name = "qw";
            if(!state.hasNumericalFluent(p, &qw)) {
                ROS_ERROR("%s: target object: %s - no qw in state.", __func__, target.c_str());
                continue;
            }

            // compute dXY, dYaw between current pose and target
            tf::Transform targetTransform(btQuaternion(qx, qy, qz, qw), btVector3(posX, posY, 0.0));
            tf::Transform deltaTransform = targetTransform.inverseTimes(transform);

            double dDist = hypot(deltaTransform.getOrigin().x(), deltaTransform.getOrigin().y());
            double dAng = tf::getYaw(deltaTransform.getRotation());
            ROS_INFO("Target %s dist: %f m ang: %f deg", target.c_str(), dDist, angles::to_degrees(dAng));

            if(!_atPredicate.empty()) {
                // Found a target - update state!
                if(dDist < _goalToleranceXY && fabs(dAng) < _goalToleranceYaw) {
                    ROS_INFO("(at) target %s !", target.c_str());
                    state.setBooleanPredicate(_atPredicate, target, true);
                    atLocations++;
                } else {
                    state.setBooleanPredicate(_atPredicate, target, false);
                }
                if(dDist < minDist) {
                    minDist = dDist;
                    nearestTarget = target;
                }
            }
        }

        ROS_INFO("Nearest target is %s (%f m).", nearestTarget.c_str(), minDist);

        // 2.a Set the robot pose, if we are not already at another pose
        if(!_atPredicate.empty() && !_robotPoseObject.empty()) {
            if(atLocations == 0) {
                state.setBooleanPredicate(_atPredicate, _robotPoseObject, true);
            } else {
                state.setBooleanPredicate(_atPredicate, _robotPoseObject, false);
                if(atLocations > 1) {
                    ROS_WARN("We are at %d locations at the same time!.", atLocations);
                }
            }
        }

        if(s_PublishLocationsAsMarkers)
            publishLocationsAsMarkers(state);

        return true;
    }

    /**
     * Publish markers for locations:
     * target locations are yellow or green if the robot is at the location
     * the robot location is white or blue if the robot is at the location
     */
    void StateCreatorRobotPose::publishLocationsAsMarkers(const SymbolicState & state)
    {
        if(!_markerPub) {
            ROS_WARN("%s: _markerPub invalid.", __func__);
            return;
        }

        visualization_msgs::MarkerArray ma;

        if(!_locationType.empty()) {
            // Check if we are at any grasp_locations
            pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> targets =
                state.getTypedObjects().equal_range(_locationType);

            vector<string> paramList;
            paramList.push_back("dummy");

            unsigned int count = 0;
            for(SymbolicState::TypedObjectConstIterator it = targets.first; it != targets.second; it++) {
                string target = it->second;
                Predicate p;
                paramList[0] = target;
                p.parameters = paramList;

                p.name = "x";
                double valueX;
                if(!state.hasNumericalFluent(p, &valueX)) {
                    ROS_ERROR("%s: target object: %s - no x-location.", __func__, target.c_str());
                    continue;
                }
                double valueY;
                p.name = "y";
                if(!state.hasNumericalFluent(p, &valueY)) {
                    ROS_ERROR("%s: target object: %s - no y-location.", __func__, target.c_str());
                    continue;
                }

                p.name = _atPredicate;
                bool at = false;
                if(!state.hasBooleanPredicate(p, &at)) {
                    ROS_ERROR("%s: state has at-base not set for %s.", __func__, target.c_str());
                    continue;
                }

                // we now know for this target it's x/y coords and if the robot is at this target
                visualization_msgs::Marker mark;
                mark.header.frame_id = "/map";
                mark.ns = "target_locations";
                mark.id = count;
                count++;
                mark.type = visualization_msgs::Marker::SPHERE;
                mark.action = visualization_msgs::Marker::ADD;
                mark.pose.position.x = valueX;
                mark.pose.position.y = valueY;
                mark.pose.position.z = 0.15;
                mark.pose.orientation.w = 1.0;
                mark.scale.x = 0.3;
                mark.scale.y = 0.3;
                mark.scale.z = 0.3;
                mark.color.a = 1.0;
                if(at) {
                    mark.color.g = 1.0;
                } else {
                    mark.color.r = 1.0;
                    mark.color.g = 1.0;
                }
                mark.text = target;
                ma.markers.push_back(mark);
            }

            // all should be overwritten as #targets is const, but to be safe
            for(unsigned int i = count; i < 100; i++) {
                visualization_msgs::Marker mark;
                mark.header.frame_id = "/map";
                mark.ns = "target_locations";
                mark.id = i;
                mark.type = visualization_msgs::Marker::SPHERE;
                mark.action = visualization_msgs::Marker::DELETE;
                ma.markers.push_back(mark);
            }
        }

        // finally robot location marker
        if(!_robotPoseObject.empty()) {
            bool l0ok = true;
            visualization_msgs::Marker mark;
            mark.header.frame_id = "/map";
            mark.ns = "robot_location";
            mark.id = 0;
            mark.type = visualization_msgs::Marker::SPHERE;
            mark.action = visualization_msgs::Marker::ADD;

            Predicate p;
            vector<string> paramList;
            paramList[0] = _robotPoseObject;
            p.parameters = paramList;

            p.name = "x";
            double valueX;
            if(!state.hasNumericalFluent(p, &valueX)) {
                ROS_ERROR("%s: %s - no x-location.", __func__, _robotPoseObject.c_str());
                l0ok = false;
            }
            double valueY;
            p.name = "y";
            if(!state.hasNumericalFluent(p, &valueY)) {
                ROS_ERROR("%s: %s - no y-location.", __func__, _robotPoseObject.c_str());
                l0ok = false;
            }

            p.name = _atPredicate;
            bool at = false;
            if(!state.hasBooleanPredicate(p, &at)) {
                ROS_ERROR("%s: state has %s not set for %s.", __func__,
                        _atPredicate.c_str(), _robotPoseObject.c_str());
                l0ok = false;
            }

            if(l0ok) {
                mark.pose.position.x = valueX;
                mark.pose.position.y = valueY;
                mark.pose.position.z = 0.15;
                mark.pose.orientation.w = 1.0;
                mark.scale.x = 0.3;
                mark.scale.y = 0.3;
                mark.scale.z = 0.3;
                mark.color.a = 1.0;
                if(at) {
                    mark.color.b = 1.0;
                } else {
                    mark.color.r = 1.0;
                    mark.color.g = 1.0;
                    mark.color.b = 1.0;
                }
                mark.text = _robotPoseObject;
            } else {
                mark.action = visualization_msgs::Marker::DELETE;
            }
            ma.markers.push_back(mark);
        }

        _markerPub.publish(ma);
    }

};

<commit_msg>stateCreatorRobotPose tries to estimate the namespace for local planner params<commit_after>#include "tidyup_actions/stateCreatorRobotPose.h"
#include <pluginlib/class_list_macros.h>
#include <visualization_msgs/MarkerArray.h>
#include <angles/angles.h>

PLUGINLIB_DECLARE_CLASS(tidyup_actions, state_creator_robot_pose,
        tidyup_actions::StateCreatorRobotPose, continual_planning_executive::StateCreator)

namespace tidyup_actions
{

    StateCreatorRobotPose::StateCreatorRobotPose()
    {
        ros::NodeHandle nhPriv("~");
        ros::NodeHandle nh;
        nhPriv.param("nav_target_tolerance_xy", _goalToleranceXY, 0.5);
        nhPriv.param("nav_target_tolerance_yaw", _goalToleranceYaw, 0.26);  //15deg

        bool relative;
        nhPriv.param("nav_target_tolerance_relative_to_move_base", relative, false);
        if(relative) {
            // relative mode: 1. get the namespace for base_local_planner
            std::string base_local_planner_ns;
            if(!nhPriv.getParam("nav_base_local_planner_ns", base_local_planner_ns)) {
                ROS_WARN("nav_target_tolerance_relative_to_move_base set, but nav_base_local_planner_ns not set - trying to estimate");
                std::string local_planner;
                if(!nh.getParam("move_base_node/base_local_planner", local_planner)) {
                    ROS_ERROR("move_base_node/base_local_planner not set - falling back to absolute mode.");
                } else {
                    // dwa_local_planner/DWAPlannerROS -> DWAPlannerROS
                    std::string::size_type x = local_planner.find_last_of("/");
                    if(x == std::string::npos)
                        base_local_planner_ns = local_planner;
                    else
                        base_local_planner_ns = local_planner.substr(x + 1);
                    ROS_INFO("Estimated base_local_planner_ns to %s.", base_local_planner_ns.c_str());
                }
            }

            if(!base_local_planner_ns.empty()) { // success: 2. get the xy_goal_tolerance
                double move_base_tol_xy;
                if(!nh.getParam(base_local_planner_ns + "/xy_goal_tolerance", move_base_tol_xy)) {
                    ROS_ERROR_STREAM("nav_target_tolerance_relative_to_move_base was true, but "
                            << (base_local_planner_ns + "/xy_goal_tolerance") << " was not set"
                            << " - falling back to absolute mode");
                } else { // 2. add move_base's tolerance to our relative tolerance
                    _goalToleranceXY += move_base_tol_xy;
                }

                double move_base_tol_yaw;
                if(!nh.getParam(base_local_planner_ns + "/yaw_goal_tolerance", move_base_tol_yaw)) {
                    ROS_ERROR_STREAM("nav_target_tolerance_relative_to_move_base was true, but "
                            << (base_local_planner_ns + "/yaw_goal_tolerance") << " was not set"
                            << " - falling back to absolute mode");
                } else { // 2. add move_base's tolerance to our relative tolerance
                    _goalToleranceYaw += move_base_tol_yaw;
                }
            }
        }

        ROS_INFO("Tolerance for accepting nav goals set to %f m, %f deg.",
                _goalToleranceXY, angles::to_degrees(_goalToleranceYaw));

        if(s_PublishLocationsAsMarkers) {
            _markerPub = nhPriv.advertise<visualization_msgs::MarkerArray>("robot_pose_markers", 5, true);
            ROS_INFO("marker topic: %s", _markerPub.getTopic().c_str());
        }
    }

    StateCreatorRobotPose::~StateCreatorRobotPose()
    {
    }

    void StateCreatorRobotPose::initialize(const std::deque<std::string> & arguments)
    {
        ROS_ASSERT(arguments.size() == 4);

        _robotPoseObject = arguments[0];
        _robotPoseType = arguments[1];
        _atPredicate = arguments[2];
        _locationType = arguments[3];

        if(_robotPoseObject == "-")
            _robotPoseObject = "";
        if(_robotPoseType == "-")
            _robotPoseType = "";
        if(_atPredicate == "-")
            _atPredicate = "";
        if(_locationType == "-")
            _locationType = "";
    }

    bool StateCreatorRobotPose::fillState(SymbolicState & state)
    {
        tf::StampedTransform transform;
        try{
            _tf.lookupTransform("/map", "/base_link", ros::Time(0), transform);
        }
        catch (tf::TransformException ex){
            ROS_ERROR("%s",ex.what());
            return false;
        }

        // 1. Real robot location
        if(!_robotPoseObject.empty()) {
            ROS_ASSERT(!_robotPoseType.empty());
            state.addObject(_robotPoseObject, _robotPoseType);
            state.setNumericalFluent("x", _robotPoseObject, transform.getOrigin().x());
            state.setNumericalFluent("y", _robotPoseObject, transform.getOrigin().y());
            state.setNumericalFluent("z", _robotPoseObject, transform.getOrigin().z());
            state.setNumericalFluent("qx", _robotPoseObject, transform.getRotation().x());
            state.setNumericalFluent("qy", _robotPoseObject, transform.getRotation().y());
            state.setNumericalFluent("qz", _robotPoseObject, transform.getRotation().z());
            state.setNumericalFluent("qw", _robotPoseObject, transform.getRotation().w());
            state.setNumericalFluent("timestamp", _robotPoseObject, ros::Time::now().toSec());
            state.addObject("/map", "frameid");
            state.setObjectFluent("frame-id", _robotPoseObject, "/map");
        }

        // 2.b check if we are at any _locations
        pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> targets =
            state.getTypedObjects().equal_range(_locationType);

        vector<string> paramList;
        paramList.push_back("dummy");

        double minDist = HUGE_VAL;
        string nearestTarget = "";

        int atLocations = 0;
        for(SymbolicState::TypedObjectConstIterator it = targets.first; it != targets.second; it++) {
            string target = it->second;
            if(target == _robotPoseObject)  // skip current robot location
                continue;

            // first get xyz, qxyzw from state
            Predicate p;
            paramList[0] = target;
            p.parameters = paramList;

            p.name = "x";
            double posX;
            if(!state.hasNumericalFluent(p, &posX)) {
                ROS_ERROR("%s: target object: %s - no x-location in state.", __func__, target.c_str());
                continue;
            }
            double posY;
            p.name = "y";
            if(!state.hasNumericalFluent(p, &posY)) {
                ROS_ERROR("%s: target object: %s - no y-location in state.", __func__, target.c_str());
                continue;
            }

            double qx;
            p.name = "qx";
            if(!state.hasNumericalFluent(p, &qx)) {
                ROS_ERROR("%s: target object: %s - no qx in state.", __func__, target.c_str());
                continue;
            }
            double qy;
            p.name = "qy";
            if(!state.hasNumericalFluent(p, &qy)) {
                ROS_ERROR("%s: target object: %s - no qy in state.", __func__, target.c_str());
                continue;
            }
            double qz;
            p.name = "qz";
            if(!state.hasNumericalFluent(p, &qz)) {
                ROS_ERROR("%s: target object: %s - no qz in state.", __func__, target.c_str());
                continue;
            }
            double qw;
            p.name = "qw";
            if(!state.hasNumericalFluent(p, &qw)) {
                ROS_ERROR("%s: target object: %s - no qw in state.", __func__, target.c_str());
                continue;
            }

            // compute dXY, dYaw between current pose and target
            tf::Transform targetTransform(btQuaternion(qx, qy, qz, qw), btVector3(posX, posY, 0.0));
            tf::Transform deltaTransform = targetTransform.inverseTimes(transform);

            double dDist = hypot(deltaTransform.getOrigin().x(), deltaTransform.getOrigin().y());
            double dAng = tf::getYaw(deltaTransform.getRotation());
            ROS_INFO("Target %s dist: %f m ang: %f deg", target.c_str(), dDist, angles::to_degrees(dAng));

            if(!_atPredicate.empty()) {
                // Found a target - update state!
                if(dDist < _goalToleranceXY && fabs(dAng) < _goalToleranceYaw) {
                    ROS_INFO("(at) target %s !", target.c_str());
                    state.setBooleanPredicate(_atPredicate, target, true);
                    atLocations++;
                } else {
                    state.setBooleanPredicate(_atPredicate, target, false);
                }
                if(dDist < minDist) {
                    minDist = dDist;
                    nearestTarget = target;
                }
            }
        }

        ROS_INFO("Nearest target is %s (%f m).", nearestTarget.c_str(), minDist);

        // 2.a Set the robot pose, if we are not already at another pose
        if(!_atPredicate.empty() && !_robotPoseObject.empty()) {
            if(atLocations == 0) {
                state.setBooleanPredicate(_atPredicate, _robotPoseObject, true);
            } else {
                state.setBooleanPredicate(_atPredicate, _robotPoseObject, false);
                if(atLocations > 1) {
                    ROS_WARN("We are at %d locations at the same time!.", atLocations);
                }
            }
        }

        if(s_PublishLocationsAsMarkers)
            publishLocationsAsMarkers(state);

        return true;
    }

    /**
     * Publish markers for locations:
     * target locations are yellow or green if the robot is at the location
     * the robot location is white or blue if the robot is at the location
     */
    void StateCreatorRobotPose::publishLocationsAsMarkers(const SymbolicState & state)
    {
        if(!_markerPub) {
            ROS_WARN("%s: _markerPub invalid.", __func__);
            return;
        }

        visualization_msgs::MarkerArray ma;

        if(!_locationType.empty()) {
            // Check if we are at any grasp_locations
            pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> targets =
                state.getTypedObjects().equal_range(_locationType);

            vector<string> paramList;
            paramList.push_back("dummy");

            unsigned int count = 0;
            for(SymbolicState::TypedObjectConstIterator it = targets.first; it != targets.second; it++) {
                string target = it->second;
                Predicate p;
                paramList[0] = target;
                p.parameters = paramList;

                p.name = "x";
                double valueX;
                if(!state.hasNumericalFluent(p, &valueX)) {
                    ROS_ERROR("%s: target object: %s - no x-location.", __func__, target.c_str());
                    continue;
                }
                double valueY;
                p.name = "y";
                if(!state.hasNumericalFluent(p, &valueY)) {
                    ROS_ERROR("%s: target object: %s - no y-location.", __func__, target.c_str());
                    continue;
                }

                p.name = _atPredicate;
                bool at = false;
                if(!state.hasBooleanPredicate(p, &at)) {
                    ROS_ERROR("%s: state has at-base not set for %s.", __func__, target.c_str());
                    continue;
                }

                // we now know for this target it's x/y coords and if the robot is at this target
                visualization_msgs::Marker mark;
                mark.header.frame_id = "/map";
                mark.ns = "target_locations";
                mark.id = count;
                count++;
                mark.type = visualization_msgs::Marker::SPHERE;
                mark.action = visualization_msgs::Marker::ADD;
                mark.pose.position.x = valueX;
                mark.pose.position.y = valueY;
                mark.pose.position.z = 0.15;
                mark.pose.orientation.w = 1.0;
                mark.scale.x = 0.3;
                mark.scale.y = 0.3;
                mark.scale.z = 0.3;
                mark.color.a = 1.0;
                if(at) {
                    mark.color.g = 1.0;
                } else {
                    mark.color.r = 1.0;
                    mark.color.g = 1.0;
                }
                mark.text = target;
                ma.markers.push_back(mark);
            }

            // all should be overwritten as #targets is const, but to be safe
            for(unsigned int i = count; i < 100; i++) {
                visualization_msgs::Marker mark;
                mark.header.frame_id = "/map";
                mark.ns = "target_locations";
                mark.id = i;
                mark.type = visualization_msgs::Marker::SPHERE;
                mark.action = visualization_msgs::Marker::DELETE;
                ma.markers.push_back(mark);
            }
        }

        // finally robot location marker
        if(!_robotPoseObject.empty()) {
            bool l0ok = true;
            visualization_msgs::Marker mark;
            mark.header.frame_id = "/map";
            mark.ns = "robot_location";
            mark.id = 0;
            mark.type = visualization_msgs::Marker::SPHERE;
            mark.action = visualization_msgs::Marker::ADD;

            Predicate p;
            vector<string> paramList;
            paramList[0] = _robotPoseObject;
            p.parameters = paramList;

            p.name = "x";
            double valueX;
            if(!state.hasNumericalFluent(p, &valueX)) {
                ROS_ERROR("%s: %s - no x-location.", __func__, _robotPoseObject.c_str());
                l0ok = false;
            }
            double valueY;
            p.name = "y";
            if(!state.hasNumericalFluent(p, &valueY)) {
                ROS_ERROR("%s: %s - no y-location.", __func__, _robotPoseObject.c_str());
                l0ok = false;
            }

            p.name = _atPredicate;
            bool at = false;
            if(!state.hasBooleanPredicate(p, &at)) {
                ROS_ERROR("%s: state has %s not set for %s.", __func__,
                        _atPredicate.c_str(), _robotPoseObject.c_str());
                l0ok = false;
            }

            if(l0ok) {
                mark.pose.position.x = valueX;
                mark.pose.position.y = valueY;
                mark.pose.position.z = 0.15;
                mark.pose.orientation.w = 1.0;
                mark.scale.x = 0.3;
                mark.scale.y = 0.3;
                mark.scale.z = 0.3;
                mark.color.a = 1.0;
                if(at) {
                    mark.color.b = 1.0;
                } else {
                    mark.color.r = 1.0;
                    mark.color.g = 1.0;
                    mark.color.b = 1.0;
                }
                mark.text = _robotPoseObject;
            } else {
                mark.action = visualization_msgs::Marker::DELETE;
            }
            ma.markers.push_back(mark);
        }

        _markerPub.publish(ma);
    }

};

<|endoftext|>


<commit_before>
#include "CGlyphNodeManager.h"
#include <ionWindow.h>

#include "CProgramContext.h"
#include "CDataSet.h"

vec3f CGlyph::GetPosition() const
{
	return Position;
}

color3f CGlyph::GetColor() const
{
	return Color;
}

void CGlyphNodeManager::Init()
{
	SingletonPointer<CSceneManager> SceneManager;

	Node = SceneManager->GetFactory()->AddSceneNode("Glyph");
}

void CGlyphNodeManager::LoadSceneElements()
{
	size_t const MaxParticles = Glyphs.size();
	PositionBuffer = new ion::GL::VertexBuffer;
	PositionBuffer->Data<f32>(MaxParticles * sizeof(f32), nullptr, 3);
	ColorBuffer = new ion::GL::VertexBuffer;
	ColorBuffer->Data<f32>(MaxParticles * sizeof(f32), nullptr, 3);

	if (Node)
	{
		Node->SetVertexBuffer("vPosition", PositionBuffer);
		Node->SetVertexBuffer("vColor", ColorBuffer);
		Node->SetUniform("Model", & Node->GetTransformationUniform());
		Node->SetPrimitiveType(ion::GL::EPrimitiveType::Points);
	}

	Positions.clear();
	Colors.clear();

	size_t const FloatsNeeded = Glyphs.size() * 3;
	if (Positions.size() < FloatsNeeded)
	{
		Positions.resize(FloatsNeeded);
		Colors.resize(FloatsNeeded);
	}

	for (uint i = 0; i < Glyphs.size(); ++ i)
	{
		Positions[i*3 + 0] = Glyphs[i]->Position.X;
		Positions[i*3 + 1] = Glyphs[i]->Position.Y;
		Positions[i*3 + 2] = Glyphs[i]->Position.Z;
		Colors[i*3 + 0] = Glyphs[i]->Color.Red;
		Colors[i*3 + 1] = Glyphs[i]->Color.Green;
		Colors[i*3 + 2] = Glyphs[i]->Color.Blue;
	}

	PositionBuffer->SubData(Positions);
	ColorBuffer->SubData(Colors);

	if (Node)
	{
		Node->SetElementCount((uint) Glyphs.size());
		Node->SetVisible(Glyphs.size() != 0);
	}

}

void CGlyphNodeManager::LoadGlyphs(CDataSet * DataSet, IColorMapper * ColorMapper)
{
	Glyphs.clear();

	ColorMapper->PreProcessValues(DataSet->Points);

	SRange<f64> XRange = DataSet->Points.GetFieldRange(DataSet->Traits.PositionXField, 15.0);
	SRange<f64> YRange = DataSet->Points.GetFieldRange(DataSet->Traits.PositionYField, 15.0);
	SRange<f64> ZRange = DataSet->Points.GetFieldRange(DataSet->Traits.PositionZField, 15.0);

	if (XRange.IsEmpty())
		XRange = SRange<f64>(-1, 1);
	if (YRange.IsEmpty())
		YRange = SRange<f64>(-1, 1);
	if (ZRange.IsEmpty())
		ZRange = SRange<f64>(-1, 1);

	printf("built in data range is %g %g to %g %g long lat\n", XRange.Minimum, ZRange.Minimum, XRange.Maximum, ZRange.Maximum);
	printf("depth varies from %g to %g\n", YRange.Minimum, YRange.Maximum);

	for (auto Point : DataSet->Points.GetValues())
	{
		CGlyph * Glyph = new CGlyph();

		f32 X = (f32) XRange.Normalize(Point.GetField(DataSet->Traits.PositionXField));
		if (XRange.IsEmpty())
			X = 0.f;

		f32 Y = (f32) YRange.Normalize(Point.GetField(DataSet->Traits.PositionYField));
		if (DataSet->Traits.InvertY)
			Y = 1.f - Y;
		if (YRange.IsEmpty())
			Y = 0.f;

		f32 Z = (f32) ZRange.Normalize(Point.GetField(DataSet->Traits.PositionZField));
		if (ZRange.IsEmpty())
			Z = 0.f;

		/*
		f64 v = it->GetField(FloorLabel);
		if (v != 0)
		{
			f32 Depth = (f32) v / (f32) YRange.second;
			Glyph.FloorHeight = 1.f - Depth;
		}
		*/

		Glyph->Position = vec3f(X, Y, Z) - 0.5f;
		Glyph->Color = ColorMapper->GetColor(Point);

		Glyphs.push_back(Glyph);
	}

	//Context->Scene.Glyphs->BuildLines();
	LoadSceneElements();
}

CSceneNode * CGlyphNodeManager::GetNode()
{
	return Node;
}

CSceneNode const * CGlyphNodeManager::GetNode() const
{
	return Node;
}
<commit_msg>Fix glyph count<commit_after>
#include "CGlyphNodeManager.h"
#include <ionWindow.h>

#include "CProgramContext.h"
#include "CDataSet.h"

vec3f CGlyph::GetPosition() const
{
	return Position;
}

color3f CGlyph::GetColor() const
{
	return Color;
}

void CGlyphNodeManager::Init()
{
	SingletonPointer<CSceneManager> SceneManager;

	Node = SceneManager->GetFactory()->AddSceneNode("Glyph");
}

void CGlyphNodeManager::LoadSceneElements()
{
	size_t const FloatsNeeded = Glyphs.size() * 3;
	PositionBuffer = new ion::GL::VertexBuffer;
	PositionBuffer->Data<f32>(FloatsNeeded * sizeof(f32), nullptr, 3);
	ColorBuffer = new ion::GL::VertexBuffer;
	ColorBuffer->Data<f32>(FloatsNeeded * sizeof(f32), nullptr, 3);

	if (Node)
	{
		Node->SetVertexBuffer("vPosition", PositionBuffer);
		Node->SetVertexBuffer("vColor", ColorBuffer);
		Node->SetUniform("Model", & Node->GetTransformationUniform());
		Node->SetPrimitiveType(ion::GL::EPrimitiveType::Points);
	}

	Positions.clear();
	Colors.clear();

	if (Positions.size() < FloatsNeeded)
	{
		Positions.resize(FloatsNeeded);
		Colors.resize(FloatsNeeded);
	}

	for (uint i = 0; i < Glyphs.size(); ++ i)
	{
		Positions[i*3 + 0] = Glyphs[i]->Position.X;
		Positions[i*3 + 1] = Glyphs[i]->Position.Y;
		Positions[i*3 + 2] = Glyphs[i]->Position.Z;
		Colors[i*3 + 0] = Glyphs[i]->Color.Red;
		Colors[i*3 + 1] = Glyphs[i]->Color.Green;
		Colors[i*3 + 2] = Glyphs[i]->Color.Blue;
	}

	PositionBuffer->SubData(Positions);
	ColorBuffer->SubData(Colors);

	if (Node)
	{
		Node->SetElementCount((uint) Glyphs.size());
		Node->SetVisible(Glyphs.size() != 0);
	}

}

void CGlyphNodeManager::LoadGlyphs(CDataSet * DataSet, IColorMapper * ColorMapper)
{
	Glyphs.clear();

	ColorMapper->PreProcessValues(DataSet->Points);

	SRange<f64> XRange = DataSet->Points.GetFieldRange(DataSet->Traits.PositionXField, 15.0);
	SRange<f64> YRange = DataSet->Points.GetFieldRange(DataSet->Traits.PositionYField, 15.0);
	SRange<f64> ZRange = DataSet->Points.GetFieldRange(DataSet->Traits.PositionZField, 15.0);

	if (XRange.IsEmpty())
		XRange = SRange<f64>(-1, 1);
	if (YRange.IsEmpty())
		YRange = SRange<f64>(-1, 1);
	if (ZRange.IsEmpty())
		ZRange = SRange<f64>(-1, 1);

	printf("built in data range is %g %g to %g %g long lat\n", XRange.Minimum, ZRange.Minimum, XRange.Maximum, ZRange.Maximum);
	printf("depth varies from %g to %g\n", YRange.Minimum, YRange.Maximum);

	for (auto Point : DataSet->Points.GetValues())
	{
		CGlyph * Glyph = new CGlyph();

		f32 X = (f32) XRange.Normalize(Point.GetField(DataSet->Traits.PositionXField));
		if (XRange.IsEmpty())
			X = 0.f;

		f32 Y = (f32) YRange.Normalize(Point.GetField(DataSet->Traits.PositionYField));
		if (DataSet->Traits.InvertY)
			Y = 1.f - Y;
		if (YRange.IsEmpty())
			Y = 0.f;

		f32 Z = (f32) ZRange.Normalize(Point.GetField(DataSet->Traits.PositionZField));
		if (ZRange.IsEmpty())
			Z = 0.f;

		/*
		f64 v = it->GetField(FloorLabel);
		if (v != 0)
		{
			f32 Depth = (f32) v / (f32) YRange.second;
			Glyph.FloorHeight = 1.f - Depth;
		}
		*/

		Glyph->Position = vec3f(X, Y, Z) - 0.5f;
		Glyph->Color = ColorMapper->GetColor(Point);

		Glyphs.push_back(Glyph);
	}

	LoadSceneElements();
}

CSceneNode * CGlyphNodeManager::GetNode()
{
	return Node;
}

CSceneNode const * CGlyphNodeManager::GetNode() const
{
	return Node;
}
<|endoftext|>


<commit_before>//
//  kern_audio.cpp
//  AppleALC
//
//  Copyright © 2018 vit9696. All rights reserved.
//

#include <Headers/kern_iokit.hpp>
#include <Headers/plugin_start.hpp>

#include "kern_audio.hpp"

OSDefineMetaClassAndStructors(AppleALCAudio, IOService)

IOService *AppleALCAudio::probe(IOService *hdaService, SInt32 *score) {
	if (!ADDPR(startSuccess))
		return nullptr;

	if (!hdaService) {
		DBGLOG("audio", "received null digitial audio device");
		return nullptr;
	}

	uint32_t hdaVen, hdaDev;
	if (!WIOKit::getOSDataValue(hdaService, "vendor-id", hdaVen) ||
		!WIOKit::getOSDataValue(hdaService, "device-id", hdaDev)) {
		SYSLOG("audio", "found an unknown device");
		return nullptr;
	}

	auto hdaPlaneName = hdaService->getName();
	DBGLOG("audio", "corrects analog audio for hdef at %s with %04X:%04X",
		   safeString(hdaPlaneName), hdaVen, hdaDev);

	if (hdaVen != WIOKit::VendorID::Intel) {
		DBGLOG("audio", "unsupported hdef vendor");
		return nullptr;
	}

	// HDEF device is always named and is not named as B0D3 or HDAU.
	bool isAnalog = hdaPlaneName && strcmp(hdaPlaneName, "B0D3") && strcmp(hdaPlaneName, "HDAU");

	if (!isAnalog) {
		DBGLOG("audio", "found digital audio, ignoring.");
		return nullptr;
	}

	// AppleHDAController only recognises HDEF and HDAU.
	if (strcmp(hdaPlaneName, "HDEF")) {
		DBGLOG("audio", "fixing audio plane name to HDEF");
		WIOKit::renameDevice(hdaService, "HDEF");
	}

	// Firstly parse our own ID.
	// alcid=X has highest priority and overrides any other value.
	// alc-layout-id has normal priority and is expected to be used.
	// layout-id will be used if both alcid and alc-layout-id are not set.
	uint32_t layout = 0;
	if (PE_parse_boot_argn("alcid", &layout, sizeof(layout))) {
		DBGLOG("audio", "found alc-layout-id override %d", layout);
		hdaService->setProperty("alc-layout-id", &layout, sizeof(layout));
	} else {
		auto alcId = OSDynamicCast(OSData, hdaService->getProperty("alc-layout-id"));
		if (alcId && alcId->getLength() == sizeof(uint32_t)) {
			DBGLOG("audio", "found normal alc-layout-id %d",
				   *static_cast<const uint32_t *>(alcId->getBytesNoCopy()));
		} else {
			auto legacyId = OSDynamicCast(OSData, hdaService->getProperty("layout-id"));
			if (legacyId && legacyId->getLength() == sizeof(uint32_t)) {
				DBGLOG("audio", "found legacy alc-layout-id (from layout-id) %d",
					   *static_cast<const uint32_t *>(alcId->getBytesNoCopy()));
				hdaService->setProperty("alc-layout-id", legacyId);
			}
		}
	}

	// Next parse the ID we will return to AppleHDA.
	// alcapplid has highest priority and overrides any other value.
	// apple-layout-id has normal priority, you may use it if you need.
	// default value (7) will be used if both of the above are not set.
	if (PE_parse_boot_argn("alcaaplid", &layout, sizeof(layout))) {
		DBGLOG("audio", "found apple-layout-id override %d", layout);
		hdaService->setProperty("apple-layout-id", &layout, sizeof(layout));
	}

	auto appleId = OSDynamicCast(OSData, hdaService->getProperty("apple-layout-id"));
	if (appleId && appleId->getLength() == sizeof(uint32_t)) {
		DBGLOG("audio", "found apple-layout-id %d",
			   *static_cast<const uint32_t *>(appleId->getBytesNoCopy()));
		hdaService->setProperty("layout-id", appleId);
	} else {
		DBGLOG("audiop", "defaulting apple-layout-id to 7");
		layout = 7;
		hdaService->setProperty("layout-id", &layout, sizeof(layout));
		hdaService->setProperty("apple-layout-id", &layout, sizeof(layout));
	}

	if (!hdaService->getProperty("built-in")) {
		DBGLOG("audio", "fixing built-in in hdef");
		uint8_t builtBytes[] { 0x00 };
		hdaService->setProperty("built-in", builtBytes, sizeof(builtBytes));
	} else {
		DBGLOG("audio", "found existing built-in in hdef");
	}

	// These seem to fix AppleHDA warnings, perhaps research them later.
	// They are probably related to the current volume of the boot bell sound.
	if (!hdaService->getProperty("MaximumBootBeepVolume")) {
		DBGLOG("audio", "fixing MaximumBootBeepVolume in hdef");
		uint8_t bootBeepBytes[] { 0xEE };
		hdaService->setProperty("MaximumBootBeepVolume", bootBeepBytes, sizeof(bootBeepBytes));
	}

	if (!hdaService->getProperty("MaximumBootBeepVolumeAlt")) {
		DBGLOG("audio", "fixing MaximumBootBeepVolumeAlt in hdef");
		uint8_t bootBeepBytes[] { 0xEE };
		hdaService->setProperty("MaximumBootBeepVolumeAlt", bootBeepBytes, sizeof(bootBeepBytes));
	}

	if (!hdaService->getProperty("PinConfigurations")) {
		DBGLOG("audio", "fixing PinConfigurations in hdef");
		uint8_t pinBytes[] { 0x00 };
		hdaService->setProperty("PinConfigurations", pinBytes, sizeof(pinBytes));
	}

	return nullptr;
}

<commit_msg>Avoid a possible null pointer dereference with -alcdbg<commit_after>//
//  kern_audio.cpp
//  AppleALC
//
//  Copyright © 2018 vit9696. All rights reserved.
//

#include <Headers/kern_iokit.hpp>
#include <Headers/plugin_start.hpp>

#include "kern_audio.hpp"

OSDefineMetaClassAndStructors(AppleALCAudio, IOService)

IOService *AppleALCAudio::probe(IOService *hdaService, SInt32 *score) {
	if (!ADDPR(startSuccess))
		return nullptr;

	if (!hdaService) {
		DBGLOG("audio", "received null digitial audio device");
		return nullptr;
	}

	uint32_t hdaVen, hdaDev;
	if (!WIOKit::getOSDataValue(hdaService, "vendor-id", hdaVen) ||
		!WIOKit::getOSDataValue(hdaService, "device-id", hdaDev)) {
		SYSLOG("audio", "found an unknown device");
		return nullptr;
	}

	auto hdaPlaneName = hdaService->getName();
	DBGLOG("audio", "corrects analog audio for hdef at %s with %04X:%04X",
		   safeString(hdaPlaneName), hdaVen, hdaDev);

	if (hdaVen != WIOKit::VendorID::Intel) {
		DBGLOG("audio", "unsupported hdef vendor");
		return nullptr;
	}

	// HDEF device is always named and is not named as B0D3 or HDAU.
	bool isAnalog = hdaPlaneName && strcmp(hdaPlaneName, "B0D3") && strcmp(hdaPlaneName, "HDAU");

	if (!isAnalog) {
		DBGLOG("audio", "found digital audio, ignoring.");
		return nullptr;
	}

	// AppleHDAController only recognises HDEF and HDAU.
	if (strcmp(hdaPlaneName, "HDEF")) {
		DBGLOG("audio", "fixing audio plane name to HDEF");
		WIOKit::renameDevice(hdaService, "HDEF");
	}

	// Firstly parse our own ID.
	// alcid=X has highest priority and overrides any other value.
	// alc-layout-id has normal priority and is expected to be used.
	// layout-id will be used if both alcid and alc-layout-id are not set.
	uint32_t layout = 0;
	if (PE_parse_boot_argn("alcid", &layout, sizeof(layout))) {
		DBGLOG("audio", "found alc-layout-id override %d", layout);
		hdaService->setProperty("alc-layout-id", &layout, sizeof(layout));
	} else {
		auto alcId = OSDynamicCast(OSData, hdaService->getProperty("alc-layout-id"));
		if (alcId && alcId->getLength() == sizeof(uint32_t)) {
			DBGLOG("audio", "found normal alc-layout-id %d",
				   *static_cast<const uint32_t *>(alcId->getBytesNoCopy()));
		} else {
			auto legacyId = OSDynamicCast(OSData, hdaService->getProperty("layout-id"));
			if (legacyId && legacyId->getLength() == sizeof(uint32_t)) {
				DBGLOG("audio", "found legacy alc-layout-id (from layout-id) %d",
					   *static_cast<const uint32_t *>(legacyId->getBytesNoCopy()));
				hdaService->setProperty("alc-layout-id", legacyId);
			}
		}
	}

	// Next parse the ID we will return to AppleHDA.
	// alcapplid has highest priority and overrides any other value.
	// apple-layout-id has normal priority, you may use it if you need.
	// default value (7) will be used if both of the above are not set.
	if (PE_parse_boot_argn("alcaaplid", &layout, sizeof(layout))) {
		DBGLOG("audio", "found apple-layout-id override %d", layout);
		hdaService->setProperty("apple-layout-id", &layout, sizeof(layout));
	}

	auto appleId = OSDynamicCast(OSData, hdaService->getProperty("apple-layout-id"));
	if (appleId && appleId->getLength() == sizeof(uint32_t)) {
		DBGLOG("audio", "found apple-layout-id %d",
			   *static_cast<const uint32_t *>(appleId->getBytesNoCopy()));
		hdaService->setProperty("layout-id", appleId);
	} else {
		DBGLOG("audiop", "defaulting apple-layout-id to 7");
		layout = 7;
		hdaService->setProperty("layout-id", &layout, sizeof(layout));
		hdaService->setProperty("apple-layout-id", &layout, sizeof(layout));
	}

	if (!hdaService->getProperty("built-in")) {
		DBGLOG("audio", "fixing built-in in hdef");
		uint8_t builtBytes[] { 0x00 };
		hdaService->setProperty("built-in", builtBytes, sizeof(builtBytes));
	} else {
		DBGLOG("audio", "found existing built-in in hdef");
	}

	// These seem to fix AppleHDA warnings, perhaps research them later.
	// They are probably related to the current volume of the boot bell sound.
	if (!hdaService->getProperty("MaximumBootBeepVolume")) {
		DBGLOG("audio", "fixing MaximumBootBeepVolume in hdef");
		uint8_t bootBeepBytes[] { 0xEE };
		hdaService->setProperty("MaximumBootBeepVolume", bootBeepBytes, sizeof(bootBeepBytes));
	}

	if (!hdaService->getProperty("MaximumBootBeepVolumeAlt")) {
		DBGLOG("audio", "fixing MaximumBootBeepVolumeAlt in hdef");
		uint8_t bootBeepBytes[] { 0xEE };
		hdaService->setProperty("MaximumBootBeepVolumeAlt", bootBeepBytes, sizeof(bootBeepBytes));
	}

	if (!hdaService->getProperty("PinConfigurations")) {
		DBGLOG("audio", "fixing PinConfigurations in hdef");
		uint8_t pinBytes[] { 0x00 };
		hdaService->setProperty("PinConfigurations", pinBytes, sizeof(pinBytes));
	}

	return nullptr;
}

<|endoftext|>


<commit_before><commit_msg>-Fixed viewer launching issue on MacOS when passing in Chrome switches<commit_after><|endoftext|>