<commit_before>// Test harness for the HtmlParser class.
#include <iostream>
#include <cstdlib>
#include "FileUtil.h"
#include "HtmlParser.h"
#include "util.h"

void Usage() {
    std::cerr << "Usage: " << ::progname << " html_filename\n";
    std::exit(EXIT_FAILURE);
}

class Parser: public HtmlParser {
public:
    Parser(const std::string &document): HtmlParser(document) { }
    virtual void notify(const HtmlParser::Chunk &chunk);
};

void Parser::notify(const HtmlParser::Chunk &chunk) {
    std::cout << chunk.toString() << '\n';
}

int main(int argc, char *argv[]) {
    ::progname = argv[1];
    if (argc != 2)
        Usage();

    try {
        const std::string input_filename(argv[1]);
        std::string html_document;
        if (not FileUtil::ReadString(input_filename, &html_document))
            ERROR("failed to read an HTML document from \"" + input_filename + "\"!");

        Parser parser(html_document);
        parser.parse();
    } catch (const std::exception &x) {
        ERROR(x.what());
    }
}
<commit_msg>Fixed an invalid default argument bug.<commit_after>// Test harness for the HtmlParser class.
#include <iostream>
#include <cstdlib>
#include "FileUtil.h"
#include "HtmlParser.h"
#include "util.h"

void Usage() {
    std::cerr << "Usage: " << ::progname << " html_filename\n";
    std::exit(EXIT_FAILURE);
}

class Parser: public HtmlParser {
public:
    Parser(const std::string &document)
        : HtmlParser(document,
                     HtmlParser::EVERYTHING & ~(HtmlParser::WORD | HtmlParser::PUNCTUATION | HtmlParser::WHITESPACE)) { }
    virtual void notify(const HtmlParser::Chunk &chunk);
};

void Parser::notify(const HtmlParser::Chunk &chunk) {
    std::cout << chunk.toString() << '\n';
}

int main(int argc, char *argv[]) {
    ::progname = argv[1];
    if (argc != 2)
        Usage();

    try {
        const std::string input_filename(argv[1]);
        std::string html_document;
        if (not FileUtil::ReadString(input_filename, &html_document))
            ERROR("failed to read an HTML document from \"" + input_filename + "\"!");

        Parser parser(html_document);
        parser.parse();
    } catch (const std::exception &x) {
        ERROR(x.what());
    }
}
<|endoftext|>


<commit_before>#include <iostream>

#include <boost/filesystem.hpp>

#include <opencv2/highgui.hpp>

#include "core/core.h"
#include "encoding/populationcode.h"
#include "io/readmnist.h"
#include "neuron/wtapoisson.h"
#include "neuron/neuron.h"
#include "neuron/zneuron.h"

using namespace cv;
using namespace std;
namespace bfs=boost::filesystem;

class Simulation
{
public:
    Simulation()
    {

    }

    void Learn()
    {
        ReadMNISTImages r;
        bfs::path p("C:\\Users\\woodstock\\dev\\data\\MNIST\\train-images.idx3-ubyte");
        r.ReadHeader(p.string().c_str());

        MutexPopulationCode pop_code;

        WTAPoisson wta_z(1.f, 1000.f);

        bool first_pass = true;

        while(!r.IS_EOF()) {

            Mat img = r.Next();
            cv::imshow("i", img);

            pop_code.State(img);

            Mat1f pc = pop_code.PopCode();

            YNeuron neuron_y;
            neuron_y.init(1.f, 1000.f);

            Mat1f spikes_y(1, pc.total());

            for(size_t i; i<pc.total(); i++) {

                spikes_y(i) = neuron_y.State(pc(i));
            }

            if(first_pass) {

                for(size_t i; i<spikes_y.total(); i++) {

                    shared_ptr<ZNeuron> p(new ZNeuron);
                    p->init(spikes_y.total(), 10);
                }
            }

            Mat1f u(1, layer_z_.size());
            for(size_t i; i<layer_z_.size(); i++) {

                u(i) = layer_z_[i]->Predict(spikes_y).at<float>(0);
            }

            Mat1f spikes_z = wta_z.Compete(layer_z_);
            for(size_t i; i<layer_z_.size(); i++) {

                layer_z_[i]->Learn(spikes_z.col(i) > 0);
            }

            if(first_pass) { first_pass = false; }
            waitKey();
        }
    }

    void Test()
    {
        ReadMNISTImages r;
        bfs::path p("C:\\Users\\woodstock\\dev\\data\\MNIST\\t10k-images.idx3-ubyte");
        r.ReadHeader(p.string().c_str());

        MutexPopulationCode pop_code;

        WTAPoisson wta_z(1.f, 1000.f);

        bool first_pass = true;

        while(!r.IS_EOF()) {

            Mat img = r.Next();
            cv::imshow("i", img);

            pop_code.State(img);

            Mat1f pc = pop_code.PopCode();

            YNeuron neuron_y;
            neuron_y.init(1.f, 1000.f);

            Mat1f spikes_y(1, pc.total());

            for(size_t i; i<pc.total(); i++) {

                spikes_y(i) = neuron_y.State(pc(i));
            }

            if(first_pass) {

                for(size_t i; i<spikes_y.total(); i++) {

                    shared_ptr<ZNeuron> p(new ZNeuron);
                    p->init(spikes_y.total(), 10);
                }
            }

            Mat1f u(1, layer_z_.size());
            for(size_t i; i<layer_z_.size(); i++) {

                u(i) = layer_z_[i]->Predict(spikes_y).at<float>(0);
            }

            Mat1f spikes_z = wta_z.Compete(layer_z_);

            if(first_pass) { first_pass = false; }
            waitKey();
        }
    }

private:
    vector<std::shared_ptr<base_Learner> > layer_z_;

};

int main() {

    std::cout<<sem::GetVersion();

    Simulation s;
    s.Learn();
    s.Test();

    return 0;
}
<commit_msg>fix for loop initializations<commit_after>#include <iostream>

#include <boost/filesystem.hpp>

#include <opencv2/highgui.hpp>

#include "core/core.h"
#include "core/mat_utils.h"
#include "encoding/populationcode.h"
#include "io/readmnist.h"
#include "neuron/wtapoisson.h"
#include "neuron/neuron.h"
#include "neuron/zneuron.h"

using namespace cv;
using namespace std;
namespace bfs=boost::filesystem;

class Simulation
{
public:
    Simulation()
        : nb_learners_(10)
    {

    }

    void Learn()
    {
        ReadMNISTImages r;
        bfs::path p("C:\\Users\\woodstock\\dev\\data\\MNIST\\train-images.idx3-ubyte");
        r.ReadHeader(p.string().c_str());

        MutexPopulationCode pop_code;

        WTAPoisson wta_z(1.f, 1000.f);

        bool first_pass = true;

        int ri=0;
        while(!r.IS_EOF()) {

            //cout<<"i"<<ri++<<endl;
            Mat img = r.Next();
            //cv::imshow("i", img);

            pop_code.State(img);
            Mat1f pc = pop_code.PopCode();

            YNeuron neuron_y;
            neuron_y.init(1.f, 1000.f);

            Mat1f spikes_y(1, pc.total());

            for(size_t i=0; i<pc.total(); i++) {

                spikes_y(i) = neuron_y.State(pc(i));
            }

            if(first_pass) {

                for(size_t i=0; i<nb_learners_; i++) {

                    shared_ptr<ZNeuron> p(new ZNeuron);
                    p->init(spikes_y.total(), 10);
                    layer_z_.push_back(p);
                }
            }

            Mat1f u(1, layer_z_.size());
            for(size_t i=0; i<layer_z_.size(); i++) {

                u(i) = layer_z_[i]->Predict(spikes_y).at<float>(0);
            }

            //cout<<"x6"<<layer_z_.size()<<endl;
            Mat1i spikes_z = wta_z.Compete(layer_z_);
            //cout<<spikes_z<<endl;
            for(size_t i=0; i<layer_z_.size(); i++) {

                //cout<<spikes_z.col(i)<<endl;
                layer_z_[i]->Learn(spikes_z.col(i));
            }

            //cout<<"x7"<<endl;
            if(first_pass) { first_pass = false; }
            //waitKey();
        }
    }

    void Test()
    {
        ReadMNISTImages r;
        bfs::path p("C:\\Users\\woodstock\\dev\\data\\MNIST\\t10k-images.idx3-ubyte");
        r.ReadHeader(p.string().c_str());

        MutexPopulationCode pop_code;

        WTAPoisson wta_z(1.f, 1000.f);

        while(!r.IS_EOF()) {

            Mat img = r.Next();
            //cv::imshow("i", img);

            pop_code.State(img);

            Mat1f pc = pop_code.PopCode();

            YNeuron neuron_y;
            neuron_y.init(1.f, 1000.f);

            Mat1f spikes_y(1, pc.total());

            for(size_t i=0; i<pc.total(); i++) {

                spikes_y(i) = neuron_y.State(pc(i));
            }

            Mat1f u(1, layer_z_.size());
            for(size_t i=0; i<layer_z_.size(); i++) {

                u(i) = layer_z_[i]->Predict(spikes_y).at<float>(0);
            }

            //Mat1f spikes_z = wta_z.Compete(layer_z_);
            //waitKey();
        }
    }

    void Eval()
    {
        for(size_t i=0; i<layer_z_.size(); i++) {

            Mat1f w = static_pointer_cast<ZNeuron>(layer_z_[i])->Weights();
            Mat1f w_on(w.size());
            Mat1f w_off(w.size());
            for(size_t i=0, j=0; i<w.total(); i+=2, j++) {

                w_on(j) = w(i);
                w_off(j) = w(i+1);
            }

            imshow("on", sem::ConvertTo8U(w_on));
            imshow("off", sem::ConvertTo8U(w_off));
            waitKey();
        }
    }

private:
    vector<std::shared_ptr<base_Learner> > layer_z_;

    int nb_learners_;   ///< no. of learners (e.g. ZNeuron)

};

int main() {

    cout<<sem::GetVersion()<<endl;

    Simulation s;

    cout<<"Learn()"<<endl;

    s.Learn();

    cout<<"Test()"<<endl;

    s.Test();

    cout<<"Eval()"<<endl;

    s.Eval();

    return 0;
}
<|endoftext|>


<commit_before><?hh //strict

/**
 * This file is part of hhpack\codegen.
 *
 * (c) Noritaka Horio <holy.shared.design@gmail.com>
 *
 * This source file is subject to the MIT license that is bundled
 * with this source code in the file LICENSE.
 */

namespace HHPack\Codegen\Cli;

use HHPack\Codegen\{
  LibraryFileGenerator,
  ClassFileGenerator,
  GenerateType,
  ClassFileGeneratable,
  OutputNamespace
};
use HHPack\Codegen\Project\{PackageClassGenerator};
use HHPack\Codegen\HackUnit\{TestClassGenerator};
use function HHPack\Getopt\{optparser, take_on, on};
use HHPack\Getopt\Parser\{OptionParser};
use HHPack\Codegen\Cli\{CodegenGenerators};
use Facebook\DefinitionFinder\{TreeParser};
use Facebook\HackCodegen\{
  HackCodegenFactory,
  HackCodegenConfig,
  CodegenFileResult
};

final class Codegen {
  const string PROGRAM_NAME = 'codegen';
  const string PROGRAM_VERSION = '0.1.0';

  private bool $help = false;
  private bool $version = false;
  private OptionParser $optionParser;

  public function __construct() {
    $this->optionParser = optparser(
      [
        on(
          ['-h', '--help'],
          'Display help message',
          () ==> {
            $this->help = true;
          },
        ),
        on(
          ['-v', '--version'],
          'Display version',
          () ==> {
            $this->version = true;
          },
        ),
      ],
    );
  }

  public function run(Traversable<string> $argv): void {
    $args = ImmVector::fromItems($argv)->skip(1);

    $remainArgs = $this->optionParser->parse($args);

    if ($this->help) {
      $this->displayHelp();
    } else if ($this->version) {
      $this->displayVersion();
    } else {
      $type = $remainArgs->at(0);
      $name = $remainArgs->at(1);
      $genType = GenerateType::assert($type);

      $this->generateBy(Pair {$genType, $name});
    }
  }

  private function generateBy(
    Pair<GenerateType, string> $generateClass,
  ): void {
    $generators = (new DevGeneratorProvider(dev_roots()))->generators();

    $generator = LibraryFileGenerator::fromItems($generators);
    $classFile = $generator->generate($generateClass);
    $result = $classFile->save();

    if ($result === CodegenFileResult::CREATE) {
      fwrite(
        STDOUT,
        sprintf("File %s is created\n", $classFile->getFileName()),
      );
    } else if ($result === CodegenFileResult::UPDATE) {
      fwrite(
        STDOUT,
        sprintf("File %s is updated\n", $classFile->getFileName()),
      );
    } else if ($result === CodegenFileResult::NONE) {
      fwrite(
        STDOUT,
        sprintf("File %s is already exists\n", $classFile->getFileName()),
      );
    }
  }

  private function displayVersion(): void {
    fwrite(
      STDOUT,
      sprintf("%s - %s\n", static::PROGRAM_NAME, static::PROGRAM_VERSION),
    );
  }

  private function displayHelp(): void {
    fwrite(
      STDOUT,
      sprintf("Usage: %s [OPTIONS] [TYPE] [NAME]\n\n", static::PROGRAM_NAME),
    );
    fwrite(STDOUT, "Arguments:\n");
    fwrite(STDOUT, "  TYPE: generate type (ex. lib, test) \n");
    fwrite(STDOUT, "  NAME: generate class name (ex. Foo\\Bar)\n\n");
    $this->optionParser->displayHelp();
  }

}
<commit_msg>v0.1.1<commit_after><?hh //strict

/**
 * This file is part of hhpack\codegen.
 *
 * (c) Noritaka Horio <holy.shared.design@gmail.com>
 *
 * This source file is subject to the MIT license that is bundled
 * with this source code in the file LICENSE.
 */

namespace HHPack\Codegen\Cli;

use HHPack\Codegen\{
  LibraryFileGenerator,
  ClassFileGenerator,
  GenerateType,
  ClassFileGeneratable,
  OutputNamespace
};
use HHPack\Codegen\Project\{PackageClassGenerator};
use HHPack\Codegen\HackUnit\{TestClassGenerator};
use function HHPack\Getopt\{optparser, take_on, on};
use HHPack\Getopt\Parser\{OptionParser};
use HHPack\Codegen\Cli\{CodegenGenerators};
use Facebook\DefinitionFinder\{TreeParser};
use Facebook\HackCodegen\{
  HackCodegenFactory,
  HackCodegenConfig,
  CodegenFileResult
};

final class Codegen {
  const string PROGRAM_NAME = 'codegen';
  const string PROGRAM_VERSION = '0.1.1';

  private bool $help = false;
  private bool $version = false;
  private OptionParser $optionParser;

  public function __construct() {
    $this->optionParser = optparser(
      [
        on(
          ['-h', '--help'],
          'Display help message',
          () ==> {
            $this->help = true;
          },
        ),
        on(
          ['-v', '--version'],
          'Display version',
          () ==> {
            $this->version = true;
          },
        ),
      ],
    );
  }

  public function run(Traversable<string> $argv): void {
    $args = ImmVector::fromItems($argv)->skip(1);

    $remainArgs = $this->optionParser->parse($args);

    if ($this->help) {
      $this->displayHelp();
    } else if ($this->version) {
      $this->displayVersion();
    } else {
      $type = $remainArgs->at(0);
      $name = $remainArgs->at(1);
      $genType = GenerateType::assert($type);

      $this->generateBy(Pair {$genType, $name});
    }
  }

  private function generateBy(
    Pair<GenerateType, string> $generateClass,
  ): void {
    $generators = (new DevGeneratorProvider(dev_roots()))->generators();

    $generator = LibraryFileGenerator::fromItems($generators);
    $classFile = $generator->generate($generateClass);
    $result = $classFile->save();

    if ($result === CodegenFileResult::CREATE) {
      fwrite(
        STDOUT,
        sprintf("File %s is created\n", $classFile->getFileName()),
      );
    } else if ($result === CodegenFileResult::UPDATE) {
      fwrite(
        STDOUT,
        sprintf("File %s is updated\n", $classFile->getFileName()),
      );
    } else if ($result === CodegenFileResult::NONE) {
      fwrite(
        STDOUT,
        sprintf("File %s is already exists\n", $classFile->getFileName()),
      );
    }
  }

  private function displayVersion(): void {
    fwrite(
      STDOUT,
      sprintf("%s - %s\n", static::PROGRAM_NAME, static::PROGRAM_VERSION),
    );
  }

  private function displayHelp(): void {
    fwrite(
      STDOUT,
      sprintf("Usage: %s [OPTIONS] [TYPE] [NAME]\n\n", static::PROGRAM_NAME),
    );
    fwrite(STDOUT, "Arguments:\n");
    fwrite(STDOUT, "  TYPE: generate type (ex. lib, test) \n");
    fwrite(STDOUT, "  NAME: generate class name (ex. Foo\\Bar)\n\n");
    $this->optionParser->displayHelp();
  }

}
<|endoftext|>


<commit_before>#include <bits/stdc++.h>

using namespace std;

// https://cp-algorithms.com/data_structures/treap.html

mt19937_64 rng(chrono::steady_clock::now().time_since_epoch().count());

struct Node {
    int key;
    int size;
    long long prio;
    Node *l, *r;

    Node(int key) : key(key), prio(rng()), size(1), l(nullptr), r(nullptr) {}

    void update() {
        size = 1 + get_size(l) + get_size(r);
    }

    static int get_size(Node *node) {
        return node ? node->size : 0;
    }
};

using pNode = Node *;

void split(pNode t, int key, pNode &l, pNode &r) {
    if (!t)
        l = r = nullptr;
    else if (key < t->key)
        split(t->l, key, l, t->l), r = t, t->update();
    else
        split(t->r, key, t->r, r), l = t, t->update();
}

void merge(pNode &t, pNode l, pNode r) {
    if (!l || !r)
        t = l ? l : r;
    else if (l->prio > r->prio)
        merge(l->r, l->r, r), t = l, t->update();
    else
        merge(r->l, l, r->l), t = r, t->update();
}

void insert(pNode &t, pNode it) {
    if (!t)
        t = it;
    else if (it->prio > t->prio)
        split(t, it->key, it->l, it->r), t = it, t->update();
    else
        insert(it->key < t->key ? t->l : t->r, it), t->update();
}

void erase(pNode &t, int key) {
    if (t->key == key) {
        pNode l = t->l;
        pNode r = t->r;
        delete t;
        merge(t, l, r);
    } else {
        erase(key < t->key ? t->l : t->r, key), t->update();
    }
}

int kth(pNode t, int k) {
    if (k < Node::get_size(t->l))
        return kth(t->l, k);
    else if (k > Node::get_size(t->l))
        return kth(t->r, k - Node::get_size(t->l) - 1);
    return t->key;
}

void print(pNode t) {
    if (!t)
        return;
    print(t->l);
    cout << t->key << endl;
    print(t->r);
}

void clear(pNode &t) {
    if (!t)
        return;
    clear(t->l);
    clear(t->r);
    delete t;
    t = nullptr;
}

// usage example
int main() {
    pNode t1 = nullptr;
    int a1[] = {1, 2};
    for (int x: a1)
        insert(t1, new Node(x));

    pNode t2 = nullptr;
    int a2[] = {7, 4, 5};
    for (int x: a2)
        insert(t2, new Node(x));

    pNode t = nullptr;
    merge(t, t1, t2);

    for (int i = 0; i < t->size; ++i) {
        cout << kth(t, i) << endl;
    }

    clear(t);
}
<commit_msg>update<commit_after>#include <bits/stdc++.h>

using namespace std;

// https://cp-algorithms.com/data_structures/treap.html

mt19937_64 rng(chrono::steady_clock::now().time_since_epoch().count());

struct Node {
    int key;
    int size;
    long long prio;
    Node *l, *r;

    Node(int key) : key(key), prio(rng()), size(1), l(nullptr), r(nullptr) {}

    void update() {
        size = 1 + get_size(l) + get_size(r);
    }

    static int get_size(Node *node) {
        return node ? node->size : 0;
    }
};

using pNode = Node *;

void split(pNode t, int key, pNode &l, pNode &r) {
    if (!t)
        l = r = nullptr;
    else if (key < t->key)
        split(t->l, key, l, t->l), r = t, t->update();
    else
        split(t->r, key, t->r, r), l = t, t->update();
}

void merge(pNode &t, pNode l, pNode r) {
    if (!l || !r)
        t = l ? l : r;
    else if (l->prio > r->prio)
        merge(l->r, l->r, r), t = l, t->update();
    else
        merge(r->l, l, r->l), t = r, t->update();
}

void insert(pNode &t, pNode it) {
    if (!t)
        t = it;
    else if (it->prio > t->prio)
        split(t, it->key, it->l, it->r), t = it, t->update();
    else
        insert(it->key < t->key ? t->l : t->r, it), t->update();
}

void erase(pNode &t, int key) {
    if (t->key == key) {
        pNode l = t->l;
        pNode r = t->r;
        delete t;
        merge(t, l, r);
    } else {
        erase(key < t->key ? t->l : t->r, key), t->update();
    }
}

pNode unite(pNode l, pNode r) {
    if (!l || !r) return l ? l : r;
    if (l->prio < r->prio) swap(l, r);
    pNode lt, rt;
    split(r, l->key, lt, rt);
    l->l = unite(l->l, lt);
    l->r = unite(l->r, rt);
    return l;
}

int kth(pNode t, int k) {
    if (k < Node::get_size(t->l))
        return kth(t->l, k);
    else if (k > Node::get_size(t->l))
        return kth(t->r, k - Node::get_size(t->l) - 1);
    return t->key;
}

void print(pNode t) {
    if (!t)
        return;
    print(t->l);
    cout << t->key << endl;
    print(t->r);
}

void clear(pNode &t) {
    if (!t)
        return;
    clear(t->l);
    clear(t->r);
    delete t;
    t = nullptr;
}

// usage example
int main() {
    pNode t1 = nullptr;
    int a1[] = {1, 2};
    for (int x: a1)
        insert(t1, new Node(x));

    pNode t2 = nullptr;
    int a2[] = {7, 4, 5};
    for (int x: a2)
        insert(t2, new Node(x));

    pNode t = nullptr;
    merge(t, t1, t2);

    for (int i = 0; i < t->size; ++i) {
        cout << kth(t, i) << endl;
    }

    clear(t);
}
<|endoftext|>


<commit_before>#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <math.h>
#include <Eigen/LU>

#include <utils/pseudo_inversion.h>
#include <utils/skew_symmetric.h>
#include <lwr_controllers/one_task_inverse_kinematics.h>

namespace lwr_controllers
{
	OneTaskInverseKinematics::OneTaskInverseKinematics() {}
	OneTaskInverseKinematics::~OneTaskInverseKinematics() {}

	bool OneTaskInverseKinematics::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
	{
		nh_ = n;

		// get URDF and name of root and tip from the parameter server
		std::string robot_description, root_name, tip_name;

		if (!ros::param::search(n.getNamespace(),"robot_description", robot_description))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No robot description (URDF) found on parameter server ("<<n.getNamespace()<<"/robot_description)");
		    return false;
		}
		if (!nh_.getParam("root_name", root_name))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No root name found on parameter server ("<<n.getNamespace()<<"/root_name)");
		    return false;
		}
		if (!nh_.getParam("tip_name", tip_name))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No tip name found on parameter server ("<<n.getNamespace()<<"/tip_name)");
		    return false;
		}

		// Get the gravity vector (direction and magnitude)
		KDL::Vector gravity_ = KDL::Vector::Zero();
		gravity_(2) = -9.81;

		// Construct an URDF model from the xml string
		std::string xml_string;

		if (n.hasParam(robot_description))
			n.getParam(robot_description.c_str(), xml_string);
		else
		{
		    ROS_ERROR("Parameter %s not set, shutting down node...", robot_description.c_str());
		    n.shutdown();
		    return false;
		}

		if (xml_string.size() == 0)
		{
			ROS_ERROR("Unable to load robot model from parameter %s",robot_description.c_str());
		    n.shutdown();
		    return false;
		}

		ROS_DEBUG("%s content\n%s", robot_description.c_str(), xml_string.c_str());

		// Get urdf model out of robot_description
		urdf::Model model;
		if (!model.initString(xml_string))
		{
		    ROS_ERROR("Failed to parse urdf file");
		    n.shutdown();
		    return false;
		}
		ROS_INFO("Successfully parsed urdf file");

		KDL::Tree kdl_tree_;
		if (!kdl_parser::treeFromUrdfModel(model, kdl_tree_))
		{
		    ROS_ERROR("Failed to construct kdl tree");
		    n.shutdown();
		    return false;
		}

		// Parsing joint limits from urdf model
		boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
    	boost::shared_ptr<const urdf::Joint> joint_;
    	joint_limits_.min.resize(kdl_tree_.getNrOfJoints());
		joint_limits_.max.resize(kdl_tree_.getNrOfJoints());
		joint_limits_.center.resize(kdl_tree_.getNrOfJoints());
		int index;

    	for (int i = 0; i < kdl_tree_.getNrOfJoints() && link_; i++)
    	{
    		joint_ = model.getJoint(link_->parent_joint->name);
    		index = kdl_tree_.getNrOfJoints() - i - 1;

    		joint_limits_.min(index) = joint_->limits->lower;
    		joint_limits_.max(index) = joint_->limits->upper;
    		joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;

    		link_ = model.getLink(link_->getParent()->name);
    	}

		// Populate the KDL chain
		if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
		{
		    ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
		    ROS_ERROR_STREAM("  "<<root_name<<" --> "<<tip_name);
		    ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
		    ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
		    ROS_ERROR_STREAM("  The segments are:");

		    KDL::SegmentMap segment_map = kdl_tree_.getSegments();
		    KDL::SegmentMap::iterator it;

		    for( it=segment_map.begin(); it != segment_map.end(); it++ )
		      ROS_ERROR_STREAM( "    "<<(*it).first);

		  	return false;
		}

		ROS_DEBUG("Number of segments: %d", kdl_chain_.getNrOfSegments());
		ROS_DEBUG("Number of joints in chain: %d", kdl_chain_.getNrOfJoints());

		// Get joint handles for all of the joints in the chain
		for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin()+1; it != kdl_chain_.segments.end(); ++it)
		{
		    joint_handles_.push_back(robot->getHandle(it->getJoint().getName()));
		    ROS_DEBUG("%s", it->getJoint().getName().c_str() );
		}

		ROS_DEBUG(" Number of joints in handle = %lu", joint_handles_.size() );

		jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
		id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
		fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
		ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
		ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_,joint_limits_.min,joint_limits_.max,*fk_pos_solver_,*ik_vel_solver_));

		joint_msr_states_.resize(kdl_chain_.getNrOfJoints());
		joint_des_states_.resize(kdl_chain_.getNrOfJoints());
		tau_cmd_.resize(kdl_chain_.getNrOfJoints());
		J_.resize(kdl_chain_.getNrOfJoints());
		PIDs_.resize(kdl_chain_.getNrOfJoints());

		sub_command_ = nh_.subscribe("command_configuration", 1, &OneTaskInverseKinematics::command_configuration, this);
		sub_gains_ = nh_.subscribe("set_gains", 1, &OneTaskInverseKinematics::set_gains, this);

		return true;
	}

	void OneTaskInverseKinematics::starting(const ros::Time& time)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    		joint_des_states_.q(i) = joint_msr_states_.q(i);
    	}

    	Kp = 60;
    	Ki = 1.2;
    	Kd = 10;

    	for (int i = 0; i < PIDs_.size(); i++)
    		PIDs_[i].initPid(Kp,Ki,Kd,0.3,-0.3);
    	ROS_INFO("PIDs gains are: Kp = %f, Ki = %f, Kd = %f",Kp,Ki,Kd);

    	// computing forward kinematics
    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

    	//Desired posture
    	x_des_ = x_;

    	cmd_flag_ = 0;
	}

	void OneTaskInverseKinematics::update(const ros::Time& time, const ros::Duration& period)
	{

		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    	}

    	if (cmd_flag_)
    	{
    		/* ANALYTIC METHOD FOR INVERSE KINEMATICS
	    	// computing Jacobian
	    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);

	    	// computing J_pinv_
	    	pseudo_inverse(J_.data,J_pinv_);

	    	// computing forward kinematics
	    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	// end-effector position/orientation error
	    	x_err_.vel = x_des_.p - x_.p;

	    	// getting quaternion from rotation matrix
	    	x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a);
	    	x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a);

	    	skew_symmetric(quat_des_.v,skew_);

	    	for (int i = 0; i < skew_.rows(); i++)
	    	{
	    		v_temp_(i) = 0.0;
	    		for (int k = 0; k < skew_.cols(); k++)
	    			v_temp_(i) += skew_(i,k)*(quat_curr_.v(k));
	    	}

	    	x_err_.rot = quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v - v_temp_;

	    	// computing q_dot
	    	for (int i = 0; i < J_pinv_.rows(); i++)
	    	{
	    		joint_des_states_.qdot(i) = 0.0;
	    		for (int k = 0; k < J_pinv_.cols(); k++)
	    			joint_des_states_.qdot(i) += J_pinv_(i,k)*x_err_(k);
	    	}

	    	// integrating q_dot -> getting q (Euler method)
	    	for (int i = 0; i < joint_handles_.size(); i++)
	    		joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i);
			*/

	    	// computing differential inverse kinematics
	    	ik_pos_solver_->CartToJnt(joint_msr_states_.q,x_des_,joint_des_states_.q);

			// computing forward kinematics
	    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	if (Equal(x_,x_des_,0.025))
	    	{
	    		ROS_INFO("On target");
	    		cmd_flag_ = 0;
	    	}
	    }

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period);
		   	joint_handles_[i].setCommand(tau_cmd_(i));
    	}
	}

	void OneTaskInverseKinematics::command_configuration(const lwr_controllers::PoseRPY::ConstPtr &msg)
	{
		KDL::Frame frame_des_;

		switch(msg->id)
		{
			case 0:
			frame_des_ = KDL::Frame(
					KDL::Rotation::RPY(msg->orientation.roll,
						 			  msg->orientation.pitch,
								 	  msg->orientation.yaw),
					KDL::Vector(msg->position.x,
								msg->position.y,
								msg->position.z));
			break;

			case 1: // position only
			frame_des_ = KDL::Frame(
				KDL::Vector(msg->position.x,
							msg->position.y,
							msg->position.z));
			break;

			case 2: // orientation only
			frame_des_ = KDL::Frame(
				KDL::Rotation::RPY(msg->orientation.roll,
				   	 			   msg->orientation.pitch,
								   msg->orientation.yaw));
			break;

			default:
			ROS_INFO("Wrong message ID");
			return;
		}

		x_des_ = frame_des_;
		cmd_flag_ = 1;
	}

	void OneTaskInverseKinematics::set_gains(const std_msgs::Float64MultiArray::ConstPtr &msg)
	{
		if(msg->data.size() == 3)
		{
			for(int i = 0; i < PIDs_.size(); i++)
				PIDs_[i].setGains(msg->data[0],msg->data[1],msg->data[2],0.3,-0.3);
			ROS_INFO("New gains set: Kp = %f, Ki = %f, Kd = %f",msg->data[0],msg->data[1],msg->data[2]);
		}
		else
			ROS_INFO("PIDs gains needed are 3 (Kp, Ki and Kd)");
	}
}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::OneTaskInverseKinematics, controller_interface::ControllerBase)
<commit_msg>Removed KDL_SOLVERIK_JL and added limits by hand<commit_after>#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <math.h>
#include <Eigen/LU>

#include <utils/pseudo_inversion.h>
#include <utils/skew_symmetric.h>
#include <lwr_controllers/one_task_inverse_kinematics.h>

namespace lwr_controllers
{
	OneTaskInverseKinematics::OneTaskInverseKinematics() {}
	OneTaskInverseKinematics::~OneTaskInverseKinematics() {}

	bool OneTaskInverseKinematics::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
	{
		nh_ = n;

		// get URDF and name of root and tip from the parameter server
		std::string robot_description, root_name, tip_name;

		if (!ros::param::search(n.getNamespace(),"robot_description", robot_description))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No robot description (URDF) found on parameter server ("<<n.getNamespace()<<"/robot_description)");
		    return false;
		}
		if (!nh_.getParam("root_name", root_name))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No root name found on parameter server ("<<n.getNamespace()<<"/root_name)");
		    return false;
		}
		if (!nh_.getParam("tip_name", tip_name))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No tip name found on parameter server ("<<n.getNamespace()<<"/tip_name)");
		    return false;
		}

		// Get the gravity vector (direction and magnitude)
		KDL::Vector gravity_ = KDL::Vector::Zero();
		gravity_(2) = -9.81;

		// Construct an URDF model from the xml string
		std::string xml_string;

		if (n.hasParam(robot_description))
			n.getParam(robot_description.c_str(), xml_string);
		else
		{
		    ROS_ERROR("Parameter %s not set, shutting down node...", robot_description.c_str());
		    n.shutdown();
		    return false;
		}

		if (xml_string.size() == 0)
		{
			ROS_ERROR("Unable to load robot model from parameter %s",robot_description.c_str());
		    n.shutdown();
		    return false;
		}

		ROS_DEBUG("%s content\n%s", robot_description.c_str(), xml_string.c_str());

		// Get urdf model out of robot_description
		urdf::Model model;
		if (!model.initString(xml_string))
		{
		    ROS_ERROR("Failed to parse urdf file");
		    n.shutdown();
		    return false;
		}
		ROS_INFO("Successfully parsed urdf file");

		KDL::Tree kdl_tree_;
		if (!kdl_parser::treeFromUrdfModel(model, kdl_tree_))
		{
		    ROS_ERROR("Failed to construct kdl tree");
		    n.shutdown();
		    return false;
		}

		// Parsing joint limits from urdf model
		boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
    	boost::shared_ptr<const urdf::Joint> joint_;
    	joint_limits_.min.resize(kdl_tree_.getNrOfJoints());
		joint_limits_.max.resize(kdl_tree_.getNrOfJoints());
		joint_limits_.center.resize(kdl_tree_.getNrOfJoints());
		int index;

    	for (int i = 0; i < kdl_tree_.getNrOfJoints() && link_; i++)
    	{
    		joint_ = model.getJoint(link_->parent_joint->name);
    		index = kdl_tree_.getNrOfJoints() - i - 1;

    		joint_limits_.min(index) = joint_->limits->lower;
    		joint_limits_.max(index) = joint_->limits->upper;
    		joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;

    		link_ = model.getLink(link_->getParent()->name);
    	}

		// Populate the KDL chain
		if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
		{
		    ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
		    ROS_ERROR_STREAM("  "<<root_name<<" --> "<<tip_name);
		    ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
		    ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
		    ROS_ERROR_STREAM("  The segments are:");

		    KDL::SegmentMap segment_map = kdl_tree_.getSegments();
		    KDL::SegmentMap::iterator it;

		    for( it=segment_map.begin(); it != segment_map.end(); it++ )
		      ROS_ERROR_STREAM( "    "<<(*it).first);

		  	return false;
		}

		ROS_DEBUG("Number of segments: %d", kdl_chain_.getNrOfSegments());
		ROS_DEBUG("Number of joints in chain: %d", kdl_chain_.getNrOfJoints());

		// Get joint handles for all of the joints in the chain
		for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin()+1; it != kdl_chain_.segments.end(); ++it)
		{
		    joint_handles_.push_back(robot->getHandle(it->getJoint().getName()));
		    ROS_DEBUG("%s", it->getJoint().getName().c_str() );
		}

		ROS_DEBUG(" Number of joints in handle = %lu", joint_handles_.size() );

		jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
		id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
		fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
		ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
		ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_,joint_limits_.min,joint_limits_.max,*fk_pos_solver_,*ik_vel_solver_));

		joint_msr_states_.resize(kdl_chain_.getNrOfJoints());
		joint_des_states_.resize(kdl_chain_.getNrOfJoints());
		tau_cmd_.resize(kdl_chain_.getNrOfJoints());
		J_.resize(kdl_chain_.getNrOfJoints());
		PIDs_.resize(kdl_chain_.getNrOfJoints());

		sub_command_ = nh_.subscribe("command_configuration", 1, &OneTaskInverseKinematics::command_configuration, this);
		sub_gains_ = nh_.subscribe("set_gains", 1, &OneTaskInverseKinematics::set_gains, this);

		return true;
	}

	void OneTaskInverseKinematics::starting(const ros::Time& time)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    		joint_des_states_.q(i) = joint_msr_states_.q(i);
    	}

    	Kp = 60;
    	Ki = 1.2;
    	Kd = 10;

    	for (int i = 0; i < PIDs_.size(); i++)
    		PIDs_[i].initPid(Kp,Ki,Kd,0.3,-0.3);
    	ROS_INFO("PIDs gains are: Kp = %f, Ki = %f, Kd = %f",Kp,Ki,Kd);

    	// computing forward kinematics
    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

    	//Desired posture
    	x_des_ = x_;

    	cmd_flag_ = 0;
	}

	void OneTaskInverseKinematics::update(const ros::Time& time, const ros::Duration& period)
	{

		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    	}

    	if (cmd_flag_)
    	{
    		// ANALYTIC METHOD FOR INVERSE KINEMATICS
	    	// computing Jacobian
	    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);

	    	// computing J_pinv_
	    	pseudo_inverse(J_.data,J_pinv_);

	    	// computing forward kinematics
	    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	// end-effector position/orientation error
	    	x_err_.vel = x_des_.p - x_.p;

	    	// getting quaternion from rotation matrix
	    	x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a);
	    	x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a);

	    	skew_symmetric(quat_des_.v,skew_);

	    	for (int i = 0; i < skew_.rows(); i++)
	    	{
	    		v_temp_(i) = 0.0;
	    		for (int k = 0; k < skew_.cols(); k++)
	    			v_temp_(i) += skew_(i,k)*(quat_curr_.v(k));
	    	}

	    	x_err_.rot = quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v - v_temp_;

	    	// computing q_dot
	    	for (int i = 0; i < J_pinv_.rows(); i++)
	    	{
	    		joint_des_states_.qdot(i) = 0.0;
	    		for (int k = 0; k < J_pinv_.cols(); k++)
	    			joint_des_states_.qdot(i) += .3*J_pinv_(i,k)*x_err_(k);

	    	}

	    	// integrating q_dot -> getting q (Euler method)
	    	for (int i = 0; i < joint_handles_.size(); i++)
	    		joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i);

        for (int i =0;  i < joint_handles_.size(); i++)
        {
          if (joint_des_states_.q(i) < joint_limits_.min(i) )
            joint_des_states_.q(i) = joint_limits_.min(i);
          if (joint_des_states_.q(i) > joint_limits_.max(i) )
            joint_des_states_.q(i) = joint_limits_.max(i);
        }

	    	// computing differential inverse kinematics
	    //	ik_pos_solver_->CartToJnt(joint_msr_states_.q,x_des_,joint_des_states_.q);

			// computing forward kinematics
	    //	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	if (Equal(x_,x_des_,0.025))
	    	{
	    		ROS_INFO("On target");
	    		cmd_flag_ = 0;
	    	}
	    }

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period);
		   	joint_handles_[i].setCommand(tau_cmd_(i));
    	}
	}

	void OneTaskInverseKinematics::command_configuration(const lwr_controllers::PoseRPY::ConstPtr &msg)
	{
		KDL::Frame frame_des_;

		switch(msg->id)
		{
			case 0:
			frame_des_ = KDL::Frame(
					KDL::Rotation::RPY(msg->orientation.roll,
						 			  msg->orientation.pitch,
								 	  msg->orientation.yaw),
					KDL::Vector(msg->position.x,
								msg->position.y,
								msg->position.z));
			break;

			case 1: // position only
			frame_des_ = KDL::Frame(
				KDL::Vector(msg->position.x,
							msg->position.y,
							msg->position.z));
			break;

			case 2: // orientation only
			frame_des_ = KDL::Frame(
				KDL::Rotation::RPY(msg->orientation.roll,
				   	 			   msg->orientation.pitch,
								   msg->orientation.yaw));
			break;

			default:
			ROS_INFO("Wrong message ID");
			return;
		}

		x_des_ = frame_des_;
		cmd_flag_ = 1;
	}

	void OneTaskInverseKinematics::set_gains(const std_msgs::Float64MultiArray::ConstPtr &msg)
	{
		if(msg->data.size() == 3)
		{
			for(int i = 0; i < PIDs_.size(); i++)
				PIDs_[i].setGains(msg->data[0],msg->data[1],msg->data[2],0.3,-0.3);
			ROS_INFO("New gains set: Kp = %f, Ki = %f, Kd = %f",msg->data[0],msg->data[1],msg->data[2]);
		}
		else
			ROS_INFO("PIDs gains needed are 3 (Kp, Ki and Kd)");
	}
}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::OneTaskInverseKinematics, controller_interface::ControllerBase)
<|endoftext|>


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

#include <cmath>

#include <Correlator.hpp>

namespace TuneBench {

CorrelatorConf::CorrelatorConf() : isa::OpenCL::KernelConf(), width(1), height(1) {}

std::string CorrelatorConf::print() const {
  return std::to_string(width) + " " + std::to_string(height) + " " + isa::OpenCL::KernelConf::print();
}

std::string * getCorrelatorOpenCL(const CorrelatorConf & conf, const std::string & dataName, const unsigned int padding, const unsigned int nrChannels, const unsigned int nrStations, const unsigned int nrSamples, const unsigned int nrPolarizations) {
  std::string * code = new std::string();

  // Begin kernel's template
  *code = "__kernel void correlator(__global const " + dataName + "4 * const restrict input, __global " + dataName + "8 * const restrict output, __global const unsigned int * const restrict cellMapX, __global const unsigned int * const restrict cellMapY) {\n"
    "const unsigned int cell = (get_group_id(0) * " + std::to_string(conf.getNrThreadsD0() * conf.getNrItemsD0()) + ") + get_local_id(0);\n"
    "const unsigned int channel = (get_group_id(2) * " + std::to_string(conf.getNrThreadsD2()) + ") + get_local_id(2);\n"
    "const unsigned int baseStationX = cellMapX[cell];\n"
    "const unsigned int baseStationY = cellMapY[cell];\n"
    "<%DEFINE_STATION%>"
    "<%DEFINE_CELL%>"
    "\n"
    "// Compute\n"
    "for ( unsigned int sample = 0; sample < " + std::to_string(nrSamples) + "; sample += " + std::to_string(conf.getNrItemsD1()) + " ) {\n"
    "<%LOAD_AND_COMPUTE%>"
    "}\n"
    "<%STORE%>"
    "}\n";
  std::string defineStationX_sTemplate = dataName + "4 sampleStationX<%STATION%>P0 = (" + dataName + "4)(0.0);\n";
  std::string defineStationY_sTemplate = dataName + "4 sampleStationY<%STATION%>P1 = (" + dataName + "4)(0.0);\n";
  std::string defineCell_sTemplate = dataName + "8 accumulator<%CELL%> = (" + dataName + "8)(0.0);\n";
  std::string loadX_sTemplate = "sampleStationX<%STATION%>P0 = input[(channel * " + std::to_string(nrStations * isa::utils::pad(nrSamples, padding / 4)) + ") + ((baseStationX + <%WIDTH%>) * " + std::to_string(isa::utils::pad(nrSamples, padding / 4)) + ") + (sample + <%OFFSETD1%>)];\n"
    "sampleStationX<%STATION%>P1 = input[(channel * " + std::to_string(nrStations * isa::utils::pad(nrSamples, padding / 4)) + ") + ((baseStationX + <%WIDTH%>) * " + std::to_string(isa::utils::pad(nrSamples, padding / 4)) + ") + (sample + <%OFFSETD1%>)];\n";
  std::string loadY_sTemplate = "sampleStationY<%STATION%>P0 = input[(channel * " + std::to_string(nrStations * isa::utils::pad(nrSamples, padding / 4)) + ") + ((baseStationY + <%HEIGHT%>) * " + std::to_string(isa::utils::pad(nrSamples, padding / 4)) + ") + (sample + <%OFFSETD1%>)];\n"
    "sampleStationY<%STATION%>P1 = input[(channel * " + std::to_string(nrStations * isa::utils::pad(nrSamples, padding / 4)) + ") + ((baseStationY + <%HEIGHT%>) * " + std::to_string(isa::utils::pad(nrSamples, padding / 4)) + ") + (sample + <%OFFSETD1%>)];\n";
  std::vector< std::string > compute_sTemplate(8);
  compute_sTemplate[0] = "accumulator<%CELL%>.s0 += (sampleStation<%STATION%>P0.x * sampleStation<%STATION%>P1.x) - (sampleStation<%STATION%>P0.y * (-sampleStation<%STATION%>P1.y));\n";
  compute_sTemplate[1] = "accumulator<%CELL%>.s1 += (sampleStation<%STATION%>P0.x * (-sampleStation<%STATION%>P1.y)) + (sampleStation<%STATION%>P0.y * sampleStation<%STATION%>P1.x);\n";
  compute_sTemplate[2] = "accumulator<%CELL%>.s2 += (sampleStation<%STATION%>P0.x * sampleStation<%STATION%>P1.z) - (sampleStation<%STATION%>P0.y * (-sampleStation<%STATION%>P1.w));\n";
  compute_sTemplate[3] = "accumulator<%CELL%>.s3 += (sampleStation<%STATION%>P0.x * (-sampleStation<%STATION%>P1.w)) + (sampleStation<%STATION%>P0.y * sampleStation<%STATION%>P1.z);\n";
  compute_sTemplate[4] = "accumulator<%CELL%>.s4 += (sampleStation<%STATION%>P0.z * sampleStation<%STATION%>P1.x) - (sampleStation<%STATION%>P0.w * (-sampleStation<%STATION%>P1.y));\n";
  compute_sTemplate[5] = "accumulator<%CELL%>.s5 += (sampleStation<%STATION%>P0.z * (-sampleStation<%STATION%>P1.y)) + (sampleStation<%STATION%>P0.w * sampleStation<%STATION%>P1.x);\n";
  compute_sTemplate[6] = "accumulator<%CELL%>.s6 += (sampleStation<%STATION%>P0.z * sampleStation<%STATION%>P1.z) - (sampleStation<%STATION%>P0.w * (-sampleStation<%STATION%>P1.w));\n";
  compute_sTemplate[7] = "accumulator<%CELL%>.s7 += (sampleStation<%STATION%>P0.z * (-sampleStation<%STATION%>P1.w)) + (sampleStation<%STATION%>P0.w * sampleStation<%STATION%>P1.z);\n";
  std::string store_sTemplate = "output[(((((stationY + <%HEIGHT%>) * (stationY + <%HEIGHT%> + 1)) / 2) + stationX + <%WIDTH%>) * " + std::to_string(nrChannels) + ") + channel] = accumulator<%CELL%>;\n";
  // End kernel's template

  std::string * defineStation_s = new std::string();
  std::string * defineCell_s = new std::string();
  std::string * loadCompute_s = new std::string();
  std::string * store_s = new std::string();
  std::string * temp = 0;
  std::string empty_s = "";

  for ( unsigned int width = 0; width < conf.getCellWidth(); width++ ) {
    std::string width_s = std::to_string(width);

    temp = isa::utils::replace(&defineStationX_sTemplate, "<%STATION%>", width_s);
    defineStation_s->append(*temp);
    delete temp;
  }
  for ( unsigned int height = 0; height < conf.getCellHeight(); height++ ) {
    std::string height_s = std::to_string(height);

    temp = isa::utils::replace(&defineStationY_sTemplate, "<%STATION%>", height_s);
    defineStation_s->append(*temp);
    delete temp;
  }
  for ( unsigned int width = 0; width < conf.getCellWidth(); width++ ) {
    for ( unsigned int height = 0; height < conf.getCellHeight(); height++ ) {
      std::string cell_s = std::to_string(width) + std::to_string(height);
      std::string width_s = std::to_string(width);
      std::string height_s = std::to_string(height);

      temp = isa::utils::replace(&defineCell_sTemplate, "<%CELL%>", cell_s);
      defineCell_s->append(*temp);
      delete temp;
      if ( width == 0 ) {
        temp = isa::utils::replace(&store_sTemplate, " + <%WIDTH%>", empty_s);
      } else {
        temp = isa::utils::replace(&store_sTemplate, "<%WIDTH%>", width_s);
      }
      if ( height == 0 ) {
        temp = isa::utils::replace(temp, " + <%HEIGHT%>", empty_s, true);
      } else {
        temp = isa::utils::replace(temp, "<%HEIGHT%>", height_s, true);
      }
      temp = isa::utils::replace(temp, "<%CELL%>", cell_s, true);
      store_s->append(*temp);
      delete temp;
    }
  }
  for ( unsigned int sample = 0; sample < conf.getNrItemsD1(); sample++ ) {
    std::string offsetD1_s = std::to_string(sample);

    for ( unsigned int width = 0; width < conf.getCellWidth(); width++ ) {
      std::string width_s = std::to_string(width);

      if ( width == 0 ) {
        temp = isa::utils::replace(&loadX_sTemplate, "+ <%WIDTH%>", empty_s);
      } else {
        temp = isa::utils::replace(&loadX_sTemplate, "<%WIDTH%>", width_s);
      }
      temp = isa::utils::replace(temp, "<%STATION%>", width_s, true);
      loadCompute_s->append(*temp);
      delete temp;
    }
    for ( unsigned int height = 0; height < conf.getCellHeight(); height++ ) {
      std::string height_s = std::to_string(height);

      if ( height == 0 ) {
        temp = isa::utils::replace(&loadY_sTemplate, "+ <%HEIGHT%>", empty_s);
      } else {
        temp = isa::utils::replace(&loadY_sTemplate, "<%HEIGHT%>", height_s);
      }
      temp = isa::utils::replace(temp, "<%STATION%>", height_s, true);
      loadCompute_s->append(*temp);
      delete temp;
    }
    loadCompute_s = isa::utils::replace(loadCompute_s, "<%OFFSETD1%>", offsetD1_s, true);
    for ( unsigned int computeStatement = 0; computeStatement < 8; computeStatement++ ) {
      for ( unsigned int width = 0; width < conf.getCellWidth(); width++ ) {
        for ( unsigned int height = 0; height < conf.getCellHeight(); height++ ) {
          std::string cell_s = std::to_string(width) + std::to_string(height);

          temp = isa::utils::replace(&compute_sTemplate[computeStatement], "<%CELL%>", cell_s);
          loadCompute_s->append(*temp);
          delete temp;
        }
      }
    }
  }

  code = isa::utils::replace(code, "<%DEFINE_STATION%>", *defineStation_s, true);
  code = isa::utils::replace(code, "<%DEFINE_CELL%>", *defineCell_s, true);
  code = isa::utils::replace(code, "<%LOAD_AND_COMPUTE%>", *loadCompute_s, true);
  code = isa::utils::replace(code, "<%STORE%>", *store_s, true);
  delete defineStation_s;
  delete defineCell_s;
  delete loadCompute_s;
  delete store_s;

  return code;
}

unsigned int generateCellMap(const CorrelatorConf & conf, std::vector< unsigned int > & cellMapX, std::vector< unsigned int > & cellMapY, const unsigned int nrStations) {
  unsigned int nrCells = 0;

  cellMapX.clear();
  cellMapY.clear();
  for ( int stationY = nrStations - conf.getCellHeight(); stationY >= 0; stationY -= conf.getCellHeight() ) {
    for ( int stationX = 0; stationX + static_cast< int >(conf.getCellWidth()) - 1 <= stationY; stationX += conf.getCellWidth() ) {
      nrCells++;
      cellMapX.push_back(stationX);
      cellMapY.push_back(stationY);
    }
  }

  return nrCells;
}

}; // TuneBench

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

#include <cmath>

#include <Correlator.hpp>

namespace TuneBench {

CorrelatorConf::CorrelatorConf() : isa::OpenCL::KernelConf(), width(1), height(1) {}

std::string CorrelatorConf::print() const {
  return std::to_string(width) + " " + std::to_string(height) + " " + isa::OpenCL::KernelConf::print();
}

std::string * getCorrelatorOpenCL(const CorrelatorConf & conf, const std::string & dataName, const unsigned int padding, const unsigned int nrChannels, const unsigned int nrStations, const unsigned int nrSamples, const unsigned int nrPolarizations) {
  std::string * code = new std::string();

  // Begin kernel's template
  *code = "__kernel void correlator(__global const " + dataName + "4 * const restrict input, __global " + dataName + "8 * const restrict output, __global const unsigned int * const restrict cellMapX, __global const unsigned int * const restrict cellMapY) {\n"
    "const unsigned int cell = (get_group_id(0) * " + std::to_string(conf.getNrThreadsD0() * conf.getNrItemsD0()) + ") + get_local_id(0);\n"
    "const unsigned int channel = (get_group_id(2) * " + std::to_string(conf.getNrThreadsD2()) + ") + get_local_id(2);\n"
    "const unsigned int baseStationX = cellMapX[cell];\n"
    "const unsigned int baseStationY = cellMapY[cell];\n"
    "<%DEFINE_STATION%>"
    "<%DEFINE_CELL%>"
    "\n"
    "// Compute\n"
    "for ( unsigned int sample = 0; sample < " + std::to_string(nrSamples) + "; sample += " + std::to_string(conf.getNrItemsD1()) + " ) {\n"
    "<%LOAD_AND_COMPUTE%>"
    "}\n"
    "<%STORE%>"
    "}\n";
  std::string defineStationX_sTemplate = dataName + "4 sampleStationX<%STATION%>P0 = (" + dataName + "4)(0.0);\n";
  std::string defineStationY_sTemplate = dataName + "4 sampleStationY<%STATION%>P1 = (" + dataName + "4)(0.0);\n";
  std::string defineCell_sTemplate = dataName + "8 accumulator<%CELL%> = (" + dataName + "8)(0.0);\n";
  std::string loadX_sTemplate = "sampleStationX<%STATION%>P0 = input[(channel * " + std::to_string(nrStations * isa::utils::pad(nrSamples, padding / 4)) + ") + ((baseStationX + <%WIDTH%>) * " + std::to_string(isa::utils::pad(nrSamples, padding / 4)) + ") + (sample + <%OFFSETD1%>)];\n"
    "sampleStationX<%STATION%>P1 = input[(channel * " + std::to_string(nrStations * isa::utils::pad(nrSamples, padding / 4)) + ") + ((baseStationX + <%WIDTH%>) * " + std::to_string(isa::utils::pad(nrSamples, padding / 4)) + ") + (sample + <%OFFSETD1%>)];\n";
  std::string loadY_sTemplate = "sampleStationY<%STATION%>P0 = input[(channel * " + std::to_string(nrStations * isa::utils::pad(nrSamples, padding / 4)) + ") + ((baseStationY + <%HEIGHT%>) * " + std::to_string(isa::utils::pad(nrSamples, padding / 4)) + ") + (sample + <%OFFSETD1%>)];\n"
    "sampleStationY<%STATION%>P1 = input[(channel * " + std::to_string(nrStations * isa::utils::pad(nrSamples, padding / 4)) + ") + ((baseStationY + <%HEIGHT%>) * " + std::to_string(isa::utils::pad(nrSamples, padding / 4)) + ") + (sample + <%OFFSETD1%>)];\n";
  std::vector< std::string > compute_sTemplate(8);
  compute_sTemplate[0] = "accumulator<%CELL%>.s0 += (sampleStation<%STATION%>P0.x * sampleStation<%STATION%>P1.x) - (sampleStation<%STATION%>P0.y * (-sampleStation<%STATION%>P1.y));\n";
  compute_sTemplate[1] = "accumulator<%CELL%>.s1 += (sampleStation<%STATION%>P0.x * (-sampleStation<%STATION%>P1.y)) + (sampleStation<%STATION%>P0.y * sampleStation<%STATION%>P1.x);\n";
  compute_sTemplate[2] = "accumulator<%CELL%>.s2 += (sampleStation<%STATION%>P0.x * sampleStation<%STATION%>P1.z) - (sampleStation<%STATION%>P0.y * (-sampleStation<%STATION%>P1.w));\n";
  compute_sTemplate[3] = "accumulator<%CELL%>.s3 += (sampleStation<%STATION%>P0.x * (-sampleStation<%STATION%>P1.w)) + (sampleStation<%STATION%>P0.y * sampleStation<%STATION%>P1.z);\n";
  compute_sTemplate[4] = "accumulator<%CELL%>.s4 += (sampleStation<%STATION%>P0.z * sampleStation<%STATION%>P1.x) - (sampleStation<%STATION%>P0.w * (-sampleStation<%STATION%>P1.y));\n";
  compute_sTemplate[5] = "accumulator<%CELL%>.s5 += (sampleStation<%STATION%>P0.z * (-sampleStation<%STATION%>P1.y)) + (sampleStation<%STATION%>P0.w * sampleStation<%STATION%>P1.x);\n";
  compute_sTemplate[6] = "accumulator<%CELL%>.s6 += (sampleStation<%STATION%>P0.z * sampleStation<%STATION%>P1.z) - (sampleStation<%STATION%>P0.w * (-sampleStation<%STATION%>P1.w));\n";
  compute_sTemplate[7] = "accumulator<%CELL%>.s7 += (sampleStation<%STATION%>P0.z * (-sampleStation<%STATION%>P1.w)) + (sampleStation<%STATION%>P0.w * sampleStation<%STATION%>P1.z);\n";
  std::string store_sTemplate = "output[(((((stationY + <%HEIGHT%>) * (stationY + <%HEIGHT%> + 1)) / 2) + stationX + <%WIDTH%>) * " + std::to_string(nrChannels) + ") + channel] = accumulator<%CELL%>;\n";
  // End kernel's template

  std::string * defineStation_s = new std::string();
  std::string * defineCell_s = new std::string();
  std::string * loadCompute_s = new std::string();
  std::string * store_s = new std::string();
  std::string * temp = 0;
  std::string empty_s = "";

  for ( unsigned int width = 0; width < conf.getCellWidth(); width++ ) {
    std::string width_s = std::to_string(width);

    temp = isa::utils::replace(&defineStationX_sTemplate, "<%STATION%>", width_s);
    defineStation_s->append(*temp);
    delete temp;
  }
  for ( unsigned int height = 0; height < conf.getCellHeight(); height++ ) {
    std::string height_s = std::to_string(height);

    temp = isa::utils::replace(&defineStationY_sTemplate, "<%STATION%>", height_s);
    defineStation_s->append(*temp);
    delete temp;
  }
  for ( unsigned int width = 0; width < conf.getCellWidth(); width++ ) {
    for ( unsigned int height = 0; height < conf.getCellHeight(); height++ ) {
      std::string cell_s = std::to_string(width) + std::to_string(height);
      std::string width_s = std::to_string(width);
      std::string height_s = std::to_string(height);

      temp = isa::utils::replace(&defineCell_sTemplate, "<%CELL%>", cell_s);
      defineCell_s->append(*temp);
      delete temp;
      if ( width == 0 ) {
        temp = isa::utils::replace(&store_sTemplate, " + <%WIDTH%>", empty_s);
      } else {
        temp = isa::utils::replace(&store_sTemplate, "<%WIDTH%>", width_s);
      }
      if ( height == 0 ) {
        temp = isa::utils::replace(temp, " + <%HEIGHT%>", empty_s, true);
      } else {
        temp = isa::utils::replace(temp, "<%HEIGHT%>", height_s, true);
      }
      temp = isa::utils::replace(temp, "<%CELL%>", cell_s, true);
      store_s->append(*temp);
      delete temp;
    }
  }
  for ( unsigned int sample = 0; sample < conf.getNrItemsD1(); sample++ ) {
    std::string offsetD1_s = std::to_string(sample);

    for ( unsigned int width = 0; width < conf.getCellWidth(); width++ ) {
      std::string width_s = std::to_string(width);

      if ( width == 0 ) {
        temp = isa::utils::replace(&loadX_sTemplate, "+ <%WIDTH%>", empty_s);
      } else {
        temp = isa::utils::replace(&loadX_sTemplate, "<%WIDTH%>", width_s);
      }
      temp = isa::utils::replace(temp, "<%STATION%>", width_s, true);
      loadCompute_s->append(*temp);
      delete temp;
    }
    for ( unsigned int height = 0; height < conf.getCellHeight(); height++ ) {
      std::string height_s = std::to_string(height);

      if ( height == 0 ) {
        temp = isa::utils::replace(&loadY_sTemplate, "+ <%HEIGHT%>", empty_s);
      } else {
        temp = isa::utils::replace(&loadY_sTemplate, "<%HEIGHT%>", height_s);
      }
      temp = isa::utils::replace(temp, "<%STATION%>", height_s, true);
      loadCompute_s->append(*temp);
      delete temp;
    }
    if ( sample == 0 ) {
      loadCompute_s = isa::utils::replace(loadCompute_s, " + <%OFFSETD1%>", empty_s, true);
    } else {
      loadCompute_s = isa::utils::replace(loadCompute_s, "<%OFFSETD1%>", offsetD1_s, true);
    }
    for ( unsigned int computeStatement = 0; computeStatement < 8; computeStatement++ ) {
      for ( unsigned int width = 0; width < conf.getCellWidth(); width++ ) {
        for ( unsigned int height = 0; height < conf.getCellHeight(); height++ ) {
          std::string cell_s = std::to_string(width) + std::to_string(height);

          temp = isa::utils::replace(&compute_sTemplate[computeStatement], "<%CELL%>", cell_s);
          loadCompute_s->append(*temp);
          delete temp;
        }
      }
    }
  }

  code = isa::utils::replace(code, "<%DEFINE_STATION%>", *defineStation_s, true);
  code = isa::utils::replace(code, "<%DEFINE_CELL%>", *defineCell_s, true);
  code = isa::utils::replace(code, "<%LOAD_AND_COMPUTE%>", *loadCompute_s, true);
  code = isa::utils::replace(code, "<%STORE%>", *store_s, true);
  delete defineStation_s;
  delete defineCell_s;
  delete loadCompute_s;
  delete store_s;

  return code;
}

unsigned int generateCellMap(const CorrelatorConf & conf, std::vector< unsigned int > & cellMapX, std::vector< unsigned int > & cellMapY, const unsigned int nrStations) {
  unsigned int nrCells = 0;

  cellMapX.clear();
  cellMapY.clear();
  for ( int stationY = nrStations - conf.getCellHeight(); stationY >= 0; stationY -= conf.getCellHeight() ) {
    for ( int stationX = 0; stationX + static_cast< int >(conf.getCellWidth()) - 1 <= stationY; stationX += conf.getCellWidth() ) {
      nrCells++;
      cellMapX.push_back(stationX);
      cellMapY.push_back(stationY);
    }
  }

  return nrCells;
}

}; // TuneBench

<|endoftext|>


<commit_before>/**
 * @function findContours_Demo.cpp
 * @brief Demo code to find contours in an image
 * @author OpenCV team
 */

#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>

using namespace cv;
using namespace std;

Mat src; Mat src_gray;
int thresh = 100;
int max_thresh = 255;
RNG rng(12345);

/// Function header
void thresh_callback(int, void* );

/**
 * @function main
 */
int main( int, char** argv )
{
  /// Load source image and convert it to gray
  src = imread( argv[1], 1 );

  /// Convert image to gray and blur it
  cvtColor( src, src_gray, COLOR_BGR2GRAY );
  blur( src_gray, src_gray, Size(3,3) );

  /// Create Window
  const char* source_window = "Source";
  namedWindow( source_window, WINDOW_AUTOSIZE );
  imshow( source_window, src );

  createTrackbar( " Canny thresh:", "Source", &thresh, max_thresh, thresh_callback );
  thresh_callback( 0, 0 );

  waitKey(0);
  return(0);
}

/**
 * @function thresh_callback
 */
void thresh_callback(int, void* )
{
  Mat canny_output;
  vector<vector<Point> > contours;
  vector<Vec4i> hierarchy;

  /// Detect edges using canny
  Canny( src_gray, canny_output, thresh, thresh*2, 3 );
  /// Find contours
  findContours( canny_output, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0) );

  /// Draw contours
  Mat drawing = Mat::zeros( canny_output.size(), CV_8UC3 );
  for( size_t i = 0; i< contours.size(); i++ )
     {
       Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
       drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, Point() );
     }

  /// Show in a window
  namedWindow( "Contours", WINDOW_AUTOSIZE );
  imshow( "Contours", drawing );
}
<commit_msg>findcontour_example check image empty<commit_after>/**
 * @function findContours_Demo.cpp
 * @brief Demo code to find contours in an image
 * @author OpenCV team
 */

#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>

using namespace cv;
using namespace std;

Mat src; Mat src_gray;
int thresh = 100;
int max_thresh = 255;
RNG rng(12345);

/// Function header
void thresh_callback(int, void* );

/**
 * @function main
 */
int main( int, char** argv )
{
  /// Load source image
  src = imread(argv[1]);
  if (src.empty())
  {
    cerr << "No image supplied ..." << endl;
    return -1;
  }

  /// Convert image to gray and blur it
  cvtColor( src, src_gray, COLOR_BGR2GRAY );
  blur( src_gray, src_gray, Size(3,3) );

  /// Create Window
  const char* source_window = "Source";
  namedWindow( source_window, WINDOW_AUTOSIZE );
  imshow( source_window, src );

  createTrackbar( " Canny thresh:", "Source", &thresh, max_thresh, thresh_callback );
  thresh_callback( 0, 0 );

  waitKey(0);
  return(0);
}

/**
 * @function thresh_callback
 */
void thresh_callback(int, void* )
{
  Mat canny_output;
  vector<vector<Point> > contours;
  vector<Vec4i> hierarchy;

  /// Detect edges using canny
  Canny( src_gray, canny_output, thresh, thresh*2, 3 );
  /// Find contours
  findContours( canny_output, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0) );

  /// Draw contours
  Mat drawing = Mat::zeros( canny_output.size(), CV_8UC3 );
  for( size_t i = 0; i< contours.size(); i++ )
     {
       Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
       drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, Point() );
     }

  /// Show in a window
  namedWindow( "Contours", WINDOW_AUTOSIZE );
  imshow( "Contours", drawing );
}
<|endoftext|>


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

#include "modules/planning/lattice/trajectory_generator/trajectory_combiner.h"

#include <algorithm>

#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/lattice/util/reference_line_matcher.h"
#include "modules/planning/math/frame_conversion/cartesian_frenet_conversion.h"

namespace apollo {
namespace planning {

using apollo::common::PathPoint;
using apollo::common::TrajectoryPoint;

DiscretizedTrajectory TrajectoryCombiner::Combine(
    const std::vector<PathPoint>& reference_line, const Curve1d& lon_trajectory,
    const Curve1d& lat_trajectory, const double init_relative_time) {
  DiscretizedTrajectory combined_trajectory;

  double s0 = lon_trajectory.Evaluate(0, 0.0);
  double s_ref_max = reference_line.back().s();

  double t_param = 0.0;
  while (t_param < FLAGS_trajectory_time_length) {
    // linear extrapolation is handled internally in LatticeTrajectory1d;
    // no worry about t_param > lon_trajectory.ParamLength() situation
    double s = lon_trajectory.Evaluate(0, t_param);
    double s_dot =
        std::max(FLAGS_lattice_epsilon, lon_trajectory.Evaluate(1, t_param));
    double s_ddot = lon_trajectory.Evaluate(2, t_param);
    if (s > s_ref_max) {
      break;
    }

    double s_param = s - s0;
    // linear extrapolation is handled internally in LatticeTrajectory1d;
    // no worry about s_param > lat_trajectory.ParamLength() situation
    double d = lat_trajectory.Evaluate(0, s_param);
    double d_prime = lat_trajectory.Evaluate(1, s_param);
    double d_pprime = lat_trajectory.Evaluate(2, s_param);

    PathPoint matched_ref_point =
        ReferenceLineMatcher::MatchToReferenceLine(reference_line, s);

    double x = 0.0;
    double y = 0.0;
    double theta = 0.0;
    double kappa = 0.0;
    double v = 0.0;
    double a = 0.0;

    const double rs = matched_ref_point.s();
    const double rx = matched_ref_point.x();
    const double ry = matched_ref_point.y();
    const double rtheta = matched_ref_point.theta();
    const double rkappa = matched_ref_point.kappa();
    const double rdkappa = matched_ref_point.dkappa();

    std::array<double, 3> s_conditions = {rs, s_dot, s_ddot};
    std::array<double, 3> d_conditions = {d, d_prime, d_pprime};
    CartesianFrenetConverter::frenet_to_cartesian(
        rs, rx, ry, rtheta, rkappa, rdkappa, s_conditions, d_conditions, &x, &y,
        &theta, &kappa, &v, &a);

    TrajectoryPoint trajectory_point;
    trajectory_point.mutable_path_point()->set_x(x);
    trajectory_point.mutable_path_point()->set_y(y);
    trajectory_point.mutable_path_point()->set_theta(theta);
    trajectory_point.mutable_path_point()->set_kappa(kappa);
    trajectory_point.set_v(v);
    trajectory_point.set_a(a);
    trajectory_point.set_relative_time(t_param + init_relative_time);

    combined_trajectory.AppendTrajectoryPoint(trajectory_point);

    t_param = t_param + FLAGS_trajectory_time_resolution;
  }
  return combined_trajectory;
}

}  // namespace planning
}  // namespace apollo
<commit_msg>Planning: [lattice] set s in trajectory point<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/planning/lattice/trajectory_generator/trajectory_combiner.h"

#include <algorithm>

#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/lattice/util/reference_line_matcher.h"
#include "modules/planning/math/frame_conversion/cartesian_frenet_conversion.h"

namespace apollo {
namespace planning {

using apollo::common::PathPoint;
using apollo::common::TrajectoryPoint;

DiscretizedTrajectory TrajectoryCombiner::Combine(
    const std::vector<PathPoint>& reference_line, const Curve1d& lon_trajectory,
    const Curve1d& lat_trajectory, const double init_relative_time) {
  DiscretizedTrajectory combined_trajectory;

  double s0 = lon_trajectory.Evaluate(0, 0.0);
  double s_ref_max = reference_line.back().s();
  double accumulated_trajectory_s = 0.0;
  PathPoint prev_trajectory_point;

  double t_param = 0.0;
  while (t_param < FLAGS_trajectory_time_length) {
    // linear extrapolation is handled internally in LatticeTrajectory1d;
    // no worry about t_param > lon_trajectory.ParamLength() situation
    double s = lon_trajectory.Evaluate(0, t_param);
    double s_dot =
        std::max(FLAGS_lattice_epsilon, lon_trajectory.Evaluate(1, t_param));
    double s_ddot = lon_trajectory.Evaluate(2, t_param);
    if (s > s_ref_max) {
      break;
    }

    double s_param = s - s0;
    // linear extrapolation is handled internally in LatticeTrajectory1d;
    // no worry about s_param > lat_trajectory.ParamLength() situation
    double d = lat_trajectory.Evaluate(0, s_param);
    double d_prime = lat_trajectory.Evaluate(1, s_param);
    double d_pprime = lat_trajectory.Evaluate(2, s_param);

    PathPoint matched_ref_point =
        ReferenceLineMatcher::MatchToReferenceLine(reference_line, s);

    double x = 0.0;
    double y = 0.0;
    double theta = 0.0;
    double kappa = 0.0;
    double v = 0.0;
    double a = 0.0;

    const double rs = matched_ref_point.s();
    const double rx = matched_ref_point.x();
    const double ry = matched_ref_point.y();
    const double rtheta = matched_ref_point.theta();
    const double rkappa = matched_ref_point.kappa();
    const double rdkappa = matched_ref_point.dkappa();

    std::array<double, 3> s_conditions = {rs, s_dot, s_ddot};
    std::array<double, 3> d_conditions = {d, d_prime, d_pprime};
    CartesianFrenetConverter::frenet_to_cartesian(
        rs, rx, ry, rtheta, rkappa, rdkappa, s_conditions, d_conditions, &x, &y,
        &theta, &kappa, &v, &a);

    if (prev_trajectory_point.has_x() && prev_trajectory_point.has_y()) {
      double delta_x = x - prev_trajectory_point.x();
      double delta_y = y - prev_trajectory_point.y();
      double delta_s = std::hypot(delta_x, delta_y);
      accumulated_trajectory_s += delta_s;
    }

    TrajectoryPoint trajectory_point;
    trajectory_point.mutable_path_point()->set_x(x);
    trajectory_point.mutable_path_point()->set_y(y);
    trajectory_point.mutable_path_point()->set_s(accumulated_trajectory_s);
    trajectory_point.mutable_path_point()->set_theta(theta);
    trajectory_point.mutable_path_point()->set_kappa(kappa);
    trajectory_point.set_v(v);
    trajectory_point.set_a(a);
    trajectory_point.set_relative_time(t_param + init_relative_time);

    combined_trajectory.AppendTrajectoryPoint(trajectory_point);

    t_param = t_param + FLAGS_trajectory_time_resolution;

    prev_trajectory_point = trajectory_point.path_point();
  }
  return combined_trajectory;
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>// Copyright 2018 Google LLC.
// Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.

#include "src/pdf/SkPDFSubsetFont.h"

#if defined(SK_USING_THIRD_PARTY_ICU)
#include "SkLoadICU.h"
#endif

#if defined(SK_PDF_USE_HARFBUZZ_SUBSET)

#include "include/private/SkTemplates.h"
#include "include/private/SkTo.h"
#include "src/utils/SkCallableTraits.h"

#include "hb.h"
#include "hb-subset.h"

template <class T, void(*P)(T*)> using resource =
    std::unique_ptr<T, SkFunctionWrapper<std::remove_pointer_t<decltype(P)>, P>>;
using HBBlob = resource<hb_blob_t, &hb_blob_destroy>;
using HBFace = resource<hb_face_t, &hb_face_destroy>;
using HBSubsetInput = resource<hb_subset_input_t, &hb_subset_input_destroy>;
using HBSet = resource<hb_set_t, &hb_set_destroy>;

static HBBlob to_blob(sk_sp<SkData> data) {
    using blob_size_t = SkCallableTraits<decltype(hb_blob_create)>::argument<1>::type;
    if (!SkTFitsIn<blob_size_t>(data->size())) {
        return nullptr;
    }
    const char* blobData = static_cast<const char*>(data->data());
    blob_size_t blobSize = SkTo<blob_size_t>(data->size());
    return HBBlob(hb_blob_create(blobData, blobSize,
                                 HB_MEMORY_MODE_READONLY,
                                 data.release(), [](void* p){ ((SkData*)p)->unref(); }));
}

static sk_sp<SkData> to_data(HBBlob blob) {
    if (!blob) {
        return nullptr;
    }
    unsigned int length;
    const char* data = hb_blob_get_data(blob.get(), &length);
    if (!data || !length) {
        return nullptr;
    }
    return SkData::MakeWithProc(data, SkToSizeT(length),
                                [](const void*, void* ctx) { hb_blob_destroy((hb_blob_t*)ctx); },
                                blob.release());
}

template<typename...> using void_t = void;
template<typename T, typename = void>
struct SkPDFHarfBuzzSubset {
    // This is the HarfBuzz 3.0 interface.
    // hb_subset_flags_t does not exist in 2.0. It isn't dependent on T, so inline the value of
    // HB_SUBSET_FLAGS_RETAIN_GIDS until 2.0 is no longer supported.
    static HBFace Make(T input, hb_face_t* face) {
        // TODO: When possible, check if a font is 'tricky' with FT_IS_TRICKY.
        // If it isn't known if a font is 'tricky', retain the hints.
        hb_subset_input_set_flags(input, 2/*HB_SUBSET_FLAGS_RETAIN_GIDS*/);
        return HBFace(hb_subset_or_fail(face, input));
    }
};
template<typename T>
struct SkPDFHarfBuzzSubset<T, void_t<
    decltype(hb_subset_input_set_retain_gids(std::declval<T>(), std::declval<bool>())),
    decltype(hb_subset_input_set_drop_hints(std::declval<T>(), std::declval<bool>())),
    decltype(hb_subset(std::declval<hb_face_t*>(), std::declval<T>()))
    >>
{
    // This is the HarfBuzz 2.0 (non-public) interface, used if it exists.
    // This code should be removed as soon as all users are migrated to the newer API.
    static HBFace Make(T input, hb_face_t* face) {
        hb_subset_input_set_retain_gids(input, true);
        // TODO: When possible, check if a font is 'tricky' with FT_IS_TRICKY.
        // If it isn't known if a font is 'tricky', retain the hints.
        hb_subset_input_set_drop_hints(input, false);
        return HBFace(hb_subset(face, input));
    }
};

static sk_sp<SkData> subset_harfbuzz(sk_sp<SkData> fontData,
                                     const SkPDFGlyphUse& glyphUsage,
                                     int ttcIndex) {
#if defined(SK_USING_THIRD_PARTY_ICU)
    if (!SkLoadICU()) {
        return nullptr;
    }
#endif
    if (!fontData) {
        return nullptr;
    }
    HBFace face(hb_face_create(to_blob(std::move(fontData)).get(), ttcIndex));
    SkASSERT(face);

    HBSubsetInput input(hb_subset_input_create_or_fail());
    SkASSERT(input);
    if (!face || !input) {
        return nullptr;
    }
    hb_set_t* glyphs = hb_subset_input_glyph_set(input.get());
    glyphUsage.getSetValues([&glyphs](unsigned gid) { hb_set_add(glyphs, gid);});

    HBFace subset = SkPDFHarfBuzzSubset<hb_subset_input_t*>::Make(input.get(), face.get());
    if (!subset) {
        return nullptr;
    }
    HBBlob result(hb_face_reference_blob(subset.get()));
    return to_data(std::move(result));
}

#endif  // defined(SK_PDF_USE_HARFBUZZ_SUBSET)

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

#if defined(SK_PDF_USE_SFNTLY)

#include "sample/chromium/font_subsetter.h"
#include <vector>

static sk_sp<SkData> subset_sfntly(sk_sp<SkData> fontData,
                                   const SkPDFGlyphUse& glyphUsage,
                                   const char* fontName,
                                   int ttcIndex) {
#if defined(SK_USING_THIRD_PARTY_ICU)
    if (!SkLoadICU()) {
        return nullptr;
    }
#endif
    // Generate glyph id array in format needed by sfntly.
    // TODO(halcanary): sfntly should take a more compact format.
    std::vector<unsigned> subset;
    glyphUsage.getSetValues([&subset](unsigned v) { subset.push_back(v); });

    unsigned char* subsetFont{nullptr};
#if defined(SK_BUILD_FOR_GOOGLE3)
    // TODO(halcanary): update SK_BUILD_FOR_GOOGLE3 to newest version of Sfntly.
    (void)ttcIndex;
    int subsetFontSize = SfntlyWrapper::SubsetFont(fontName,
                                                   fontData->bytes(),
                                                   fontData->size(),
                                                   subset.data(),
                                                   subset.size(),
                                                   &subsetFont);
#else  // defined(SK_BUILD_FOR_GOOGLE3)
    (void)fontName;
    int subsetFontSize = SfntlyWrapper::SubsetFont(ttcIndex,
                                                   fontData->bytes(),
                                                   fontData->size(),
                                                   subset.data(),
                                                   subset.size(),
                                                   &subsetFont);
#endif  // defined(SK_BUILD_FOR_GOOGLE3)
    SkASSERT(subsetFontSize > 0 || subsetFont == nullptr);
    if (subsetFontSize < 1 || subsetFont == nullptr) {
        return nullptr;
    }
    return SkData::MakeWithProc(subsetFont, subsetFontSize,
                                [](const void* p, void*) { delete[] (unsigned char*)p; },
                                nullptr);
}

#endif  // defined(SK_PDF_USE_SFNTLY)

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

#if defined(SK_PDF_USE_SFNTLY) && defined(SK_PDF_USE_HARFBUZZ_SUBSET)

sk_sp<SkData> SkPDFSubsetFont(sk_sp<SkData> fontData,
                              const SkPDFGlyphUse& glyphUsage,
                              SkPDF::Metadata::Subsetter subsetter,
                              const char* fontName,
                              int ttcIndex) {
    switch (subsetter) {
        case SkPDF::Metadata::kHarfbuzz_Subsetter:
            return subset_harfbuzz(std::move(fontData), glyphUsage, ttcIndex);
        case SkPDF::Metadata::kSfntly_Subsetter:
            return subset_sfntly(std::move(fontData), glyphUsage, fontName, ttcIndex);
    }
    return nullptr;
}

#elif defined(SK_PDF_USE_SFNTLY)

sk_sp<SkData> SkPDFSubsetFont(sk_sp<SkData> fontData,
                              const SkPDFGlyphUse& glyphUsage,
                              SkPDF::Metadata::Subsetter,
                              const char* fontName,
                              int ttcIndex) {
    return subset_sfntly(std::move(fontData), glyphUsage, fontName, ttcIndex);
}

#elif defined(SK_PDF_USE_HARFBUZZ_SUBSET)

sk_sp<SkData> SkPDFSubsetFont(sk_sp<SkData> fontData,
                              const SkPDFGlyphUse& glyphUsage,
                              SkPDF::Metadata::Subsetter,
                              const char*,
                              int ttcIndex) {
    return subset_harfbuzz(std::move(fontData), glyphUsage, ttcIndex);
}

#else

sk_sp<SkData> SkPDFSubsetFont(sk_sp<SkData>, const SkPDFGlyphUse&, SkPDF::Metadata::Subsetter,
                              const char*, int) {
    return nullptr;
}
#endif  // defined(SK_PDF_USE_SFNTLY)
<commit_msg>Remove ICU check from subset_harfbuzz<commit_after>// Copyright 2018 Google LLC.
// Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.

#include "src/pdf/SkPDFSubsetFont.h"

#if defined(SK_PDF_USE_HARFBUZZ_SUBSET)

#include "include/private/SkTemplates.h"
#include "include/private/SkTo.h"
#include "src/utils/SkCallableTraits.h"

#include "hb.h"
#include "hb-subset.h"

template <class T, void(*P)(T*)> using resource =
    std::unique_ptr<T, SkFunctionWrapper<std::remove_pointer_t<decltype(P)>, P>>;
using HBBlob = resource<hb_blob_t, &hb_blob_destroy>;
using HBFace = resource<hb_face_t, &hb_face_destroy>;
using HBSubsetInput = resource<hb_subset_input_t, &hb_subset_input_destroy>;
using HBSet = resource<hb_set_t, &hb_set_destroy>;

static HBBlob to_blob(sk_sp<SkData> data) {
    using blob_size_t = SkCallableTraits<decltype(hb_blob_create)>::argument<1>::type;
    if (!SkTFitsIn<blob_size_t>(data->size())) {
        return nullptr;
    }
    const char* blobData = static_cast<const char*>(data->data());
    blob_size_t blobSize = SkTo<blob_size_t>(data->size());
    return HBBlob(hb_blob_create(blobData, blobSize,
                                 HB_MEMORY_MODE_READONLY,
                                 data.release(), [](void* p){ ((SkData*)p)->unref(); }));
}

static sk_sp<SkData> to_data(HBBlob blob) {
    if (!blob) {
        return nullptr;
    }
    unsigned int length;
    const char* data = hb_blob_get_data(blob.get(), &length);
    if (!data || !length) {
        return nullptr;
    }
    return SkData::MakeWithProc(data, SkToSizeT(length),
                                [](const void*, void* ctx) { hb_blob_destroy((hb_blob_t*)ctx); },
                                blob.release());
}

template<typename...> using void_t = void;
template<typename T, typename = void>
struct SkPDFHarfBuzzSubset {
    // This is the HarfBuzz 3.0 interface.
    // hb_subset_flags_t does not exist in 2.0. It isn't dependent on T, so inline the value of
    // HB_SUBSET_FLAGS_RETAIN_GIDS until 2.0 is no longer supported.
    static HBFace Make(T input, hb_face_t* face) {
        // TODO: When possible, check if a font is 'tricky' with FT_IS_TRICKY.
        // If it isn't known if a font is 'tricky', retain the hints.
        hb_subset_input_set_flags(input, 2/*HB_SUBSET_FLAGS_RETAIN_GIDS*/);
        return HBFace(hb_subset_or_fail(face, input));
    }
};
template<typename T>
struct SkPDFHarfBuzzSubset<T, void_t<
    decltype(hb_subset_input_set_retain_gids(std::declval<T>(), std::declval<bool>())),
    decltype(hb_subset_input_set_drop_hints(std::declval<T>(), std::declval<bool>())),
    decltype(hb_subset(std::declval<hb_face_t*>(), std::declval<T>()))
    >>
{
    // This is the HarfBuzz 2.0 (non-public) interface, used if it exists.
    // This code should be removed as soon as all users are migrated to the newer API.
    static HBFace Make(T input, hb_face_t* face) {
        hb_subset_input_set_retain_gids(input, true);
        // TODO: When possible, check if a font is 'tricky' with FT_IS_TRICKY.
        // If it isn't known if a font is 'tricky', retain the hints.
        hb_subset_input_set_drop_hints(input, false);
        return HBFace(hb_subset(face, input));
    }
};

static sk_sp<SkData> subset_harfbuzz(sk_sp<SkData> fontData,
                                     const SkPDFGlyphUse& glyphUsage,
                                     int ttcIndex) {
    if (!fontData) {
        return nullptr;
    }
    HBFace face(hb_face_create(to_blob(std::move(fontData)).get(), ttcIndex));
    SkASSERT(face);

    HBSubsetInput input(hb_subset_input_create_or_fail());
    SkASSERT(input);
    if (!face || !input) {
        return nullptr;
    }
    hb_set_t* glyphs = hb_subset_input_glyph_set(input.get());
    glyphUsage.getSetValues([&glyphs](unsigned gid) { hb_set_add(glyphs, gid);});

    HBFace subset = SkPDFHarfBuzzSubset<hb_subset_input_t*>::Make(input.get(), face.get());
    if (!subset) {
        return nullptr;
    }
    HBBlob result(hb_face_reference_blob(subset.get()));
    return to_data(std::move(result));
}

#endif  // defined(SK_PDF_USE_HARFBUZZ_SUBSET)

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

#if defined(SK_PDF_USE_SFNTLY)

#include "sample/chromium/font_subsetter.h"
#include <vector>

static sk_sp<SkData> subset_sfntly(sk_sp<SkData> fontData,
                                   const SkPDFGlyphUse& glyphUsage,
                                   const char* fontName,
                                   int ttcIndex) {
#if defined(SK_USING_THIRD_PARTY_ICU)
    if (!SkLoadICU()) {
        return nullptr;
    }
#endif
    // Generate glyph id array in format needed by sfntly.
    // TODO(halcanary): sfntly should take a more compact format.
    std::vector<unsigned> subset;
    glyphUsage.getSetValues([&subset](unsigned v) { subset.push_back(v); });

    unsigned char* subsetFont{nullptr};
#if defined(SK_BUILD_FOR_GOOGLE3)
    // TODO(halcanary): update SK_BUILD_FOR_GOOGLE3 to newest version of Sfntly.
    (void)ttcIndex;
    int subsetFontSize = SfntlyWrapper::SubsetFont(fontName,
                                                   fontData->bytes(),
                                                   fontData->size(),
                                                   subset.data(),
                                                   subset.size(),
                                                   &subsetFont);
#else  // defined(SK_BUILD_FOR_GOOGLE3)
    (void)fontName;
    int subsetFontSize = SfntlyWrapper::SubsetFont(ttcIndex,
                                                   fontData->bytes(),
                                                   fontData->size(),
                                                   subset.data(),
                                                   subset.size(),
                                                   &subsetFont);
#endif  // defined(SK_BUILD_FOR_GOOGLE3)
    SkASSERT(subsetFontSize > 0 || subsetFont == nullptr);
    if (subsetFontSize < 1 || subsetFont == nullptr) {
        return nullptr;
    }
    return SkData::MakeWithProc(subsetFont, subsetFontSize,
                                [](const void* p, void*) { delete[] (unsigned char*)p; },
                                nullptr);
}

#endif  // defined(SK_PDF_USE_SFNTLY)

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

#if defined(SK_PDF_USE_SFNTLY) && defined(SK_PDF_USE_HARFBUZZ_SUBSET)

sk_sp<SkData> SkPDFSubsetFont(sk_sp<SkData> fontData,
                              const SkPDFGlyphUse& glyphUsage,
                              SkPDF::Metadata::Subsetter subsetter,
                              const char* fontName,
                              int ttcIndex) {
    switch (subsetter) {
        case SkPDF::Metadata::kHarfbuzz_Subsetter:
            return subset_harfbuzz(std::move(fontData), glyphUsage, ttcIndex);
        case SkPDF::Metadata::kSfntly_Subsetter:
            return subset_sfntly(std::move(fontData), glyphUsage, fontName, ttcIndex);
    }
    return nullptr;
}

#elif defined(SK_PDF_USE_SFNTLY)

sk_sp<SkData> SkPDFSubsetFont(sk_sp<SkData> fontData,
                              const SkPDFGlyphUse& glyphUsage,
                              SkPDF::Metadata::Subsetter,
                              const char* fontName,
                              int ttcIndex) {
    return subset_sfntly(std::move(fontData), glyphUsage, fontName, ttcIndex);
}

#elif defined(SK_PDF_USE_HARFBUZZ_SUBSET)

sk_sp<SkData> SkPDFSubsetFont(sk_sp<SkData> fontData,
                              const SkPDFGlyphUse& glyphUsage,
                              SkPDF::Metadata::Subsetter,
                              const char*,
                              int ttcIndex) {
    return subset_harfbuzz(std::move(fontData), glyphUsage, ttcIndex);
}

#else

sk_sp<SkData> SkPDFSubsetFont(sk_sp<SkData>, const SkPDFGlyphUse&, SkPDF::Metadata::Subsetter,
                              const char*, int) {
    return nullptr;
}
#endif  // defined(SK_PDF_USE_SFNTLY)
<|endoftext|>