<commit_before>#include "commands.h"

namespace commands {

std::random_device random_device;
std::default_random_engine random_engine{random_device()};

int hardware_threads = std::thread::hardware_concurrency();

bool verbose_grid{true};
int delay_ms{50};

// clang-format off
std::vector<command_structure> commands = {
  {"?",       "?",               "?", nullptr, &help,          "Print this help"},
  {"print",   "(p)rint",         "p", &print_grid, nullptr,    "Print current grid"},
  {"clear",   "(c)lear",         "c", &clear_grid, nullptr,    "Clear the grid"},
  {"resize",  "resi(z)e n",      "z", &resize_grid, nullptr,   "Set grid size to n x n and clear"},
  {"rules",   "rules b s",       "",  &set_rules, nullptr,     "Set game rules (Born, Stay Alive)"},
  {"set",     "(s)et x y",       "s", &set_cell, nullptr,      "Set cell at x y alive"},
  {"unset",   "(u)nset x y",     "u", &unset_cell, nullptr,    "Set cell at x y dead"},
  {"fill",    "(f)ill n",        "f", &fill_grid, nullptr,     "Set up to n cells alive"},
  {"threads", "(t)hreads n",     "t", &set_threads, nullptr,   "Set the amount of threads"},
  {"glider",  "(g)lider x y",    "g", &create_glider, nullptr, "Place a glider at x y"},
  {"load",    "(l)oad file x y", "l", &load_pattern, nullptr,  "Load pattern from file to x y"},
  {"export",  "e(x)port file",   "x", &export_cells, nullptr,  "Export a file to cells format"},
  {"quiet",   "(q)uiet",         "q", nullptr, &quiet,         "Disable output during running"},
  {"verbose", "(v)erbose",       "v", nullptr, &verbose,       "Enable output during running"},
  {"delay",   "(d)elay n",       "d", nullptr, &delay,         "Wait for n milliseconds after each step"},
  {"run",     "(r)un n",         "r", &run, nullptr,           "Run n iterations"},
  {"step",    "st(e)p",          "e", &step, nullptr,          "Run 1 iteration"},
  {"exit",    "exit",            "",  nullptr, &pexit,         "Exit"}
};
// clang-format on

constexpr int max_command_width{16};

bool command(std::string command, gol::Game_of_life &g) {
  for (auto c : commands) {
    if (command == c.name || command == c.shortcut) {
      if (c.function_g != nullptr)
        return c.function_g(g);
      else
        return c.function_w();
    }
  }

  std::cout << "Unknown command.\n";
  return true;
}

void print_command_name(const std::string &name) {
  std::cout.width(max_command_width);
  std::cout << name;
}

bool quiet() {
  verbose_grid = false;
  std::cout << "Grid output is now disabled.\n";
  return true;
}

bool verbose() {
  verbose_grid = true;
  std::cout << "Grid output is now enabled.\n";
  return true;
}

bool delay() {
  int n;
  if (ui::input_int(n, true, true))
    delay_ms = n;
  else
    std::cout << "Invalid value.\n";

  std::cout << "Every step will now be shown for " << delay_ms
            << " milliseconds.\n";
  return true;
}

bool help() {
  std::cout << " === Help ===\n";
  for (auto c : commands) {
    print_command_name(c.help);
    std::cout << " | " << c.description << "\n";
  }

  std::cout << "\nCoordinates are 0-based. (0 0) is top left corner.\n";
  std::cout << "\nCurrent loading the following file formats are supported:\n";
  std::cout << " * Life 1.06 (.lif or .life)\n";
  std::cout << " * Plaintext (.cells)\n";
  std::cout << " * RLE (.rle)\n";
  std::cout << "Please make sure the grid is large enough before loading.\n";

  if (hardware_threads > 1) {
    std::cout << "\nUse as many threads as possible (up to " << gol::max_threads
              << ")\nif you have more than one CPU core.\nYour CPU supports "
              << hardware_threads << " hardware threads.\n";
  }

  std::cout << "\n";
  return true;
}

bool print_grid(gol::Game_of_life &g) {
  std::cout << g;
  return true;
}

bool clear_grid(gol::Game_of_life &g) {
  g.clear();
  std::cout << "Grid cleared.\n";
  return true;
}

bool resize_grid(gol::Game_of_life &g) {
  int size;
  if (ui::input_int(size, true, false)) {
    g.size(size);
    std::cout << "New grid size is " << g.size() << ".\n";
  } else {
    std::cout << "Invalid grid size entered.\n";
  }
  return true;
}

bool set_rules(gol::Game_of_life &g) {
  std::string born;
  std::string stay_alive;

  if (std::cin >> born && std::cin >> stay_alive && util::digit_string(born) &&
      util::digit_string(stay_alive)) {
    g.clear_rules();
    // This looks ugly... sorry :/
    for (char c : born) {
      g.add_born(c - '0');
    }
    for (char c : stay_alive) {
      g.add_stay_alive(c - '0');
    }
  } else {
    std::cout << "Invalid rules. Rules must only contain digits.\n";
  }

  return true;
}

void input_set_cell(gol::Game_of_life &g, bool value) {
  int x, y;
  if (ui::input_int(x, true, true) && ui::input_int(y, true, true)) {
    if (!g.cell(x, y, value)) {
      std::cout << "Invalid coordinates.\n";
    }
  } else {
    std::cout << "Could not parse coordinates.\n";
  }
}

bool set_cell(gol::Game_of_life &g) {
  input_set_cell(g, true);

  return true;
}

bool unset_cell(gol::Game_of_life &g) {
  input_set_cell(g, false);

  return true;
}

bool fill_grid(gol::Game_of_life &g) {
  int n;
  if (ui::input_int(n, true)) {
    std::uniform_int_distribution<int> grid_distribution(0, g.size() - 1);
    for (int i = 0; i < n; i++) {
      int x = grid_distribution(random_engine);
      int y = grid_distribution(random_engine);

      g.cell(x, y, true);
    }
    std::cout << "Grid filled with up to " << n << " alive cells.\n";
  } else {
    std::cout << "Invalid number.\n";
  }
  return true;
}

bool set_threads(gol::Game_of_life &g) {
  int n;
  if (ui::input_int(n, true)) {
    g.threads(n);
    std::cout << "Set to " << g.threads() << " threads.\n";
  } else {
    std::cout << "Invalid number.\n";
  }
  return true;
}

bool create_glider(gol::Game_of_life &g) {
  int x, y;
  if (ui::input_int(x, false, true) && ui::input_int(y, false, true)) {
    g.cell(g.shift_coord(x + 1), g.shift_coord(y), true);
    g.cell(g.shift_coord(x + 2), g.shift_coord(y + 1), true);
    g.cell(g.shift_coord(x + 2), g.shift_coord(y + 2), true);
    g.cell(g.shift_coord(x + 1), g.shift_coord(y + 2), true);
    g.cell(g.shift_coord(x), g.shift_coord(y + 2), true);
    std::cout << "Glider placed.\n";
  } else {
    std::cout << "Invalid coordinates.\n";
  }
  return true;
}

bool load_pattern(gol::Game_of_life &g) {
  std::string filename{""};
  int x, y;

  if (std::cin >> filename && ui::input_int(x, true, true) &&
      ui::input_int(y, true, true)) {
    if (g.load_pattern(filename, x, y)) {
      std::cout << "OK\n";
    }
  } else {
    std::cin.clear();
    std::cin.ignore();
    std::cout << "Invalid parameters.\n";
  }
  return true;
}

bool export_cells(gol::Game_of_life &g) {
  std::string filename{""};

  if (std::cin >> filename && util::file_extension(filename) == "cells") {
    if (import_export::export_cells(filename, g)) {
      std::cout << "Exported to file.\n";
    } else {
      std::cout << "File could not be written.\n";
    }
  } else {
    std::cout << "Invalid filename (has to end with .cells)\n";
  }
  return true;
}

bool run(gol::Game_of_life &g) {
  int n;
  if (ui::input_int(n, true)) {
    for (int i = 0; i < n; i++) {
      if (!g.step()) {
        std::cout << "Aborted after " << (i + 1) << " step"
                  << (i > 0 ? "s" : "")
                  << " because of no change comprared do previous step.\n";
        break;
      };
      if (verbose_grid) {
        std::cout << "Step " << (i + 1) << ":\n";
        std::cout << g << std::endl;
        if (delay_ms) {
          std::chrono::milliseconds wait_for(delay_ms);
          std::this_thread::sleep_for(wait_for);
        }
      }
    }
  } else {
    std::cout << "Invalid number of steps.\n";
  }
  return true;
}

bool step(gol::Game_of_life &g) {
  g.step();
  if (verbose_grid) std::cout << g << std::endl;
  return true;
}

bool pexit() { return false; }
} // namespace commands
<commit_msg>Swapped input of rules to conform with standard definition<commit_after>#include "commands.h"

namespace commands {

std::random_device random_device;
std::default_random_engine random_engine{random_device()};

int hardware_threads = std::thread::hardware_concurrency();

bool verbose_grid{true};
int delay_ms{50};

// clang-format off
std::vector<command_structure> commands = {
  {"?",       "?",               "?", nullptr, &help,          "Print this help"},
  {"print",   "(p)rint",         "p", &print_grid, nullptr,    "Print current grid"},
  {"clear",   "(c)lear",         "c", &clear_grid, nullptr,    "Clear the grid"},
  {"resize",  "resi(z)e n",      "z", &resize_grid, nullptr,   "Set grid size to n x n and clear"},
  {"rules",   "rules s b",       "",  &set_rules, nullptr,     "Set game rules (Stay Alive, Born)"},
  {"set",     "(s)et x y",       "s", &set_cell, nullptr,      "Set cell at x y alive"},
  {"unset",   "(u)nset x y",     "u", &unset_cell, nullptr,    "Set cell at x y dead"},
  {"fill",    "(f)ill n",        "f", &fill_grid, nullptr,     "Set up to n cells alive"},
  {"threads", "(t)hreads n",     "t", &set_threads, nullptr,   "Set the amount of threads"},
  {"glider",  "(g)lider x y",    "g", &create_glider, nullptr, "Place a glider at x y"},
  {"load",    "(l)oad file x y", "l", &load_pattern, nullptr,  "Load pattern from file to x y"},
  {"export",  "e(x)port file",   "x", &export_cells, nullptr,  "Export a file to cells format"},
  {"quiet",   "(q)uiet",         "q", nullptr, &quiet,         "Disable output during running"},
  {"verbose", "(v)erbose",       "v", nullptr, &verbose,       "Enable output during running"},
  {"delay",   "(d)elay n",       "d", nullptr, &delay,         "Wait for n milliseconds after each step"},
  {"run",     "(r)un n",         "r", &run, nullptr,           "Run n iterations"},
  {"step",    "st(e)p",          "e", &step, nullptr,          "Run 1 iteration"},
  {"exit",    "exit",            "",  nullptr, &pexit,         "Exit"}
};
// clang-format on

constexpr int max_command_width{16};

bool command(std::string command, gol::Game_of_life &g) {
  for (auto c : commands) {
    if (command == c.name || command == c.shortcut) {
      if (c.function_g != nullptr)
        return c.function_g(g);
      else
        return c.function_w();
    }
  }

  std::cout << "Unknown command.\n";
  return true;
}

void print_command_name(const std::string &name) {
  std::cout.width(max_command_width);
  std::cout << name;
}

bool quiet() {
  verbose_grid = false;
  std::cout << "Grid output is now disabled.\n";
  return true;
}

bool verbose() {
  verbose_grid = true;
  std::cout << "Grid output is now enabled.\n";
  return true;
}

bool delay() {
  int n;
  if (ui::input_int(n, true, true))
    delay_ms = n;
  else
    std::cout << "Invalid value.\n";

  std::cout << "Every step will now be shown for " << delay_ms
            << " milliseconds.\n";
  return true;
}

bool help() {
  std::cout << " === Help ===\n";
  for (auto c : commands) {
    print_command_name(c.help);
    std::cout << " | " << c.description << "\n";
  }

  std::cout << "\nCoordinates are 0-based. (0 0) is top left corner.\n";
  std::cout << "\nCurrent loading the following file formats are supported:\n";
  std::cout << " * Life 1.06 (.lif or .life)\n";
  std::cout << " * Plaintext (.cells)\n";
  std::cout << " * RLE (.rle)\n";
  std::cout << "Please make sure the grid is large enough before loading.\n";

  if (hardware_threads > 1) {
    std::cout << "\nUse as many threads as possible (up to " << gol::max_threads
              << ")\nif you have more than one CPU core.\nYour CPU supports "
              << hardware_threads << " hardware threads.\n";
  }

  std::cout << "\n";
  return true;
}

bool print_grid(gol::Game_of_life &g) {
  std::cout << g;
  return true;
}

bool clear_grid(gol::Game_of_life &g) {
  g.clear();
  std::cout << "Grid cleared.\n";
  return true;
}

bool resize_grid(gol::Game_of_life &g) {
  int size;
  if (ui::input_int(size, true, false)) {
    g.size(size);
    std::cout << "New grid size is " << g.size() << ".\n";
  } else {
    std::cout << "Invalid grid size entered.\n";
  }
  return true;
}

bool set_rules(gol::Game_of_life &g) {
  std::string stay_alive;
  std::string born;

  if (std::cin >> stay_alive && std::cin >> born &&
      util::digit_string(stay_alive) && util::digit_string(born)) {
    g.clear_rules();
    // This looks ugly... sorry :/
    for (char c : stay_alive) {
      g.add_stay_alive(c - '0');
    }
    for (char c : born) {
      g.add_born(c - '0');
    }
  } else {
    std::cout << "Invalid rules. Rules must only contain digits.\n";
  }

  return true;
}

void input_set_cell(gol::Game_of_life &g, bool value) {
  int x, y;
  if (ui::input_int(x, true, true) && ui::input_int(y, true, true)) {
    if (!g.cell(x, y, value)) {
      std::cout << "Invalid coordinates.\n";
    }
  } else {
    std::cout << "Could not parse coordinates.\n";
  }
}

bool set_cell(gol::Game_of_life &g) {
  input_set_cell(g, true);

  return true;
}

bool unset_cell(gol::Game_of_life &g) {
  input_set_cell(g, false);

  return true;
}

bool fill_grid(gol::Game_of_life &g) {
  int n;
  if (ui::input_int(n, true)) {
    std::uniform_int_distribution<int> grid_distribution(0, g.size() - 1);
    for (int i = 0; i < n; i++) {
      int x = grid_distribution(random_engine);
      int y = grid_distribution(random_engine);

      g.cell(x, y, true);
    }
    std::cout << "Grid filled with up to " << n << " alive cells.\n";
  } else {
    std::cout << "Invalid number.\n";
  }
  return true;
}

bool set_threads(gol::Game_of_life &g) {
  int n;
  if (ui::input_int(n, true)) {
    g.threads(n);
    std::cout << "Set to " << g.threads() << " threads.\n";
  } else {
    std::cout << "Invalid number.\n";
  }
  return true;
}

bool create_glider(gol::Game_of_life &g) {
  int x, y;
  if (ui::input_int(x, false, true) && ui::input_int(y, false, true)) {
    g.cell(g.shift_coord(x + 1), g.shift_coord(y), true);
    g.cell(g.shift_coord(x + 2), g.shift_coord(y + 1), true);
    g.cell(g.shift_coord(x + 2), g.shift_coord(y + 2), true);
    g.cell(g.shift_coord(x + 1), g.shift_coord(y + 2), true);
    g.cell(g.shift_coord(x), g.shift_coord(y + 2), true);
    std::cout << "Glider placed.\n";
  } else {
    std::cout << "Invalid coordinates.\n";
  }
  return true;
}

bool load_pattern(gol::Game_of_life &g) {
  std::string filename{""};
  int x, y;

  if (std::cin >> filename && ui::input_int(x, true, true) &&
      ui::input_int(y, true, true)) {
    if (g.load_pattern(filename, x, y)) {
      std::cout << "OK\n";
    }
  } else {
    std::cin.clear();
    std::cin.ignore();
    std::cout << "Invalid parameters.\n";
  }
  return true;
}

bool export_cells(gol::Game_of_life &g) {
  std::string filename{""};

  if (std::cin >> filename && util::file_extension(filename) == "cells") {
    if (import_export::export_cells(filename, g)) {
      std::cout << "Exported to file.\n";
    } else {
      std::cout << "File could not be written.\n";
    }
  } else {
    std::cout << "Invalid filename (has to end with .cells)\n";
  }
  return true;
}

bool run(gol::Game_of_life &g) {
  int n;
  if (ui::input_int(n, true)) {
    for (int i = 0; i < n; i++) {
      if (!g.step()) {
        std::cout << "Aborted after " << (i + 1) << " step"
                  << (i > 0 ? "s" : "")
                  << " because of no change comprared do previous step.\n";
        break;
      };
      if (verbose_grid) {
        std::cout << "Step " << (i + 1) << ":\n";
        std::cout << g << std::endl;
        if (delay_ms) {
          std::chrono::milliseconds wait_for(delay_ms);
          std::this_thread::sleep_for(wait_for);
        }
      }
    }
  } else {
    std::cout << "Invalid number of steps.\n";
  }
  return true;
}

bool step(gol::Game_of_life &g) {
  g.step();
  if (verbose_grid) std::cout << g << std::endl;
  return true;
}

bool pexit() { return false; }
} // namespace commands
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <ros/forwards.h>
#include <ros/single_subscriber_publisher.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <visualization_msgs/Marker.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/cv_bridge.h>

#include <src/TagDetector.h>
#include <src/TagDetection.h>
#include <src/TagFamily.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <visualization_msgs/MarkerArray.h>
#include "yaml-cpp/yaml.h"
#include <sstream>
#include <fstream>

#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include <boost/make_shared.hpp>

#include "apriltags.h"

using namespace std;

// Functions

double GetTagSize(int tag_id)
{
    boost::unordered_map<size_t, double>::iterator tag_sizes_it =
        tag_sizes_.find(tag_id);
    if(tag_sizes_it != tag_sizes_.end()) {
        return tag_sizes_it->second;
    } else {
        return default_tag_size_;
    }
}

Eigen::Matrix4d GetDetectionTransform(TagDetection detection)
{
    double tag_size = GetTagSize(detection.id);

    std::vector<cv::Point3f> object_pts;
    std::vector<cv::Point2f> image_pts;
    double tag_radius = tag_size/2.;

    object_pts.push_back(cv::Point3f(-tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius,  tag_radius, 0));
    object_pts.push_back(cv::Point3f(-tag_radius,  tag_radius, 0));

    image_pts.push_back(detection.p[0]);
    image_pts.push_back(detection.p[1]);
    image_pts.push_back(detection.p[2]);
    image_pts.push_back(detection.p[3]);

    cv::Matx33f intrinsics(camera_info_.K[0], 0, camera_info_.K[2],
                           0, camera_info_.K[4], camera_info_.K[5],
                           0, 0, 1);

    cv::Mat rvec, tvec;
    cv::Vec4f dist_param(0,0,0,0);
    cv::solvePnP(object_pts, image_pts, intrinsics, dist_param,
            rvec, tvec);
    cv::Matx33d r;
    cv::Rodrigues(rvec, r);
    Eigen::Matrix3d rot;
    rot << r(0,0), r(0,1), r(0,2),
           r(1,0), r(1,1), r(1,2),
           r(2,0), r(2,1), r(2,2);

    Eigen::Matrix4d T;
    T.topLeftCorner(3,3) = rot;
    T.col(3).head(3) <<
            tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
    T.row(3) << 0,0,0,1;

    return T;
}

// Callback for camera info
void InfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info)
{
    camera_info_ = (*camera_info);
    has_camera_info_ = true;
}

// Callback for image data
void ImageCallback(const sensor_msgs::ImageConstPtr& msg )
{
    if(!has_camera_info_){
        ROS_WARN("No Camera Info Received Yet");
        return;
    }

    // Get the image
    cv_bridge::CvImagePtr subscribed_ptr;
    try
    {
        subscribed_ptr = cv_bridge::toCvCopy(msg, "mono8");
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat subscribed_gray = subscribed_ptr->image;
    cv::Mat tmp;
    cv::Point2d opticalCenter(0.5*subscribed_gray.rows,
                              0.5*subscribed_gray.cols);
    TagDetectionArray detections;
    detector_->process(subscribed_gray, opticalCenter, detections);
    visualization_msgs::MarkerArray marker_transforms;

    if(viewer_)
    {
        subscribed_gray = family_->superimposeDetections(subscribed_gray,
                                                         detections);
    }
    for(unsigned int i = 0; i < detections.size(); ++i)
    {
        Eigen::Matrix4d pose = GetDetectionTransform(detections[i]);

        // Get this info from earlier code, don't extract it again
        Eigen::Matrix3d R = pose.block<3,3>(0,0);
        Eigen::Quaternion<double> q(R);

        visualization_msgs::Marker marker_transform;
        marker_transform.header.frame_id = frame_;
        marker_transform.header.stamp = ros::Time::now();
        stringstream convert;
        convert << "tag" << detections[i].id;
        marker_transform.ns = convert.str().c_str();
        marker_transform.id = detections[i].id;
        marker_transform.type = visualization_msgs::Marker::ARROW;
        //marker_transform.type = visualization_msgs::Marker::CUBE;
        marker_transform.action = visualization_msgs::Marker::ADD;
        marker_transform.pose.position.x = pose(0,3);
        marker_transform.pose.position.y = pose(1,3);
        marker_transform.pose.position.z = pose(2,3);
        marker_transform.pose.orientation.x = q.x();
        marker_transform.pose.orientation.y = q.y();
        marker_transform.pose.orientation.z = q.z();
        marker_transform.pose.orientation.w = q.w();

    	double tag_size = GetTagSize(detections[i].id);
        /*
        marker_transform.scale.x = tag_size;
        marker_transform.scale.y = tag_size;
        marker_transform.scale.z = 0.01 * tag_size;
        */
        marker_transform.scale.x = tag_size*50;
        marker_transform.scale.y = tag_size*10;
        marker_transform.scale.z = tag_size*0.5;

        marker_transform.color.r = 1.0;
        marker_transform.color.g = 0.0;
        marker_transform.color.b = 1.0;
        marker_transform.color.a = 1.0;
        marker_transforms.markers.push_back(marker_transform);
    }
    marker_publisher_.publish(marker_transforms);

    if(viewer_)
    {
        cv::imshow("AprilTags", subscribed_gray);
    }
}

void ConnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers();
    ROS_DEBUG("Subscription detected! (%d subscribers)", subscribers);

    if(subscribers && !running_)
    {
        ROS_DEBUG("New Subscribers, Connecting to Input Image Topic.");
        ros::TransportHints ros_transport_hints(ros::TransportHints().tcpNoDelay());
        image_transport::TransportHints image_transport_hint(image_transport::TransportHints(
                                "raw", ros_transport_hints, (*node_),
                                "image_transport"));

        image_subscriber = (*image_).subscribe(
                DEFAULT_IMAGE_TOPIC, 1, &ImageCallback,
                image_transport_hint);
        info_subscriber = (*node_).subscribe(
                DEFAULT_CAMERA_INFO_TOPIC, 10, &InfoCallback);
        running_ = true;
    }
}

void DisconnectHandler()
{
}

void DisconnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers();
    ROS_DEBUG("Unsubscription detected! (%d subscribers)", subscribers);

    if(!subscribers && running_)
    {
        ROS_DEBUG("No Subscribers, Disconnecting from Input Image Topic.");
		image_subscriber.shutdown();
		info_subscriber.shutdown();
		running_ = false;
    }
}

void GetParameterValues()
{
    // Load node-wide configuration values.
    node_->param("viewer", viewer_, 0);
    node_->param("tag_family", tag_family_name_, DEFAULT_TAG_FAMILY);
    node_->param("default_tag_size", default_tag_size_, DEFAULT_TAG_SIZE);
    node_->param("tf_frame", frame_, DEFAULT_TF_FRAME);

    // Load tag specific configuration values.
    XmlRpc::XmlRpcValue tag_data;
    node_->param("tag_data", tag_data, tag_data);

    // Iterate through each tag in the configuration.
    XmlRpc::XmlRpcValue::ValueStruct::iterator it;
    for (it = tag_data.begin(); it != tag_data.end(); ++it)
    {
        // Retrieve the settings for the next tag.
        int tag_id = boost::lexical_cast<int>(it->first);
        XmlRpc::XmlRpcValue tag_values = it->second;

        // Load all the settings for this tag.
        if (tag_values.hasMember("size"))
        {
            tag_sizes_[tag_id] = static_cast<double>(tag_values["size"]);
            ROS_DEBUG("Setting tag%d to size %f m.", tag_id, tag_sizes_[tag_id]);
        }
    }
}

void SetupPublisher()
{
    ros::SubscriberStatusCallback connect_callback = &ConnectCallback;
    ros::SubscriberStatusCallback disconnect_callback = &DisconnectCallback;

    // Publisher
    marker_publisher_ = node_->advertise<visualization_msgs::MarkerArray>(
            DEFAULT_MARKER_TOPIC, 1, connect_callback,
            disconnect_callback);
}

void InitializeTags()
{
    tag_params.newQuadAlgorithm = 1;
    family_ = new TagFamily(tag_family_name_);
    detector_ = new TagDetector(*family_, tag_params);
}

void InitializeROSNode(int argc, char **argv)
{
    ros::init(argc, argv, "apriltags");
    node_ =  boost::make_shared<ros::NodeHandle>("~");
    image_ = boost::make_shared<image_transport::ImageTransport>(*node_);
}

int main(int argc, char **argv)
{
    InitializeROSNode(argc,argv);
    GetParameterValues();
    SetupPublisher();
    InitializeTags();

    if(viewer_){
        cvNamedWindow("AprilTags");
        cvStartWindowThread();
    }

    ROS_INFO("AprilTags node started.");
    running_ = false;
    has_camera_info_ = false;
    ros::spin();
    ROS_INFO("AprilTags node stopped.");

    //Destroying Stuff
    cvDestroyWindow("AprilTags");
    delete detector_;
    delete family_;

    return EXIT_SUCCESS;
}
<commit_msg>testing sizes<commit_after>#include <ros/ros.h>
#include <ros/forwards.h>
#include <ros/single_subscriber_publisher.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <visualization_msgs/Marker.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/cv_bridge.h>

#include <src/TagDetector.h>
#include <src/TagDetection.h>
#include <src/TagFamily.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <visualization_msgs/MarkerArray.h>
#include "yaml-cpp/yaml.h"
#include <sstream>
#include <fstream>

#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include <boost/make_shared.hpp>

#include "apriltags.h"

using namespace std;

// Functions

double GetTagSize(int tag_id)
{
    boost::unordered_map<size_t, double>::iterator tag_sizes_it =
        tag_sizes_.find(tag_id);
    if(tag_sizes_it != tag_sizes_.end()) {
        return tag_sizes_it->second;
    } else {
        return default_tag_size_;
    }
}

Eigen::Matrix4d GetDetectionTransform(TagDetection detection)
{
    double tag_size = GetTagSize(detection.id);

    std::vector<cv::Point3f> object_pts;
    std::vector<cv::Point2f> image_pts;
    double tag_radius = tag_size/2.;

    object_pts.push_back(cv::Point3f(-tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius,  tag_radius, 0));
    object_pts.push_back(cv::Point3f(-tag_radius,  tag_radius, 0));

    image_pts.push_back(detection.p[0]);
    image_pts.push_back(detection.p[1]);
    image_pts.push_back(detection.p[2]);
    image_pts.push_back(detection.p[3]);

    cv::Matx33f intrinsics(camera_info_.K[0], 0, camera_info_.K[2],
                           0, camera_info_.K[4], camera_info_.K[5],
                           0, 0, 1);

    cv::Mat rvec, tvec;
    cv::Vec4f dist_param(0,0,0,0);
    cv::solvePnP(object_pts, image_pts, intrinsics, dist_param,
            rvec, tvec);
    cv::Matx33d r;
    cv::Rodrigues(rvec, r);
    Eigen::Matrix3d rot;
    rot << r(0,0), r(0,1), r(0,2),
           r(1,0), r(1,1), r(1,2),
           r(2,0), r(2,1), r(2,2);

    Eigen::Matrix4d T;
    T.topLeftCorner(3,3) = rot;
    T.col(3).head(3) <<
            tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
    T.row(3) << 0,0,0,1;

    return T;
}

// Callback for camera info
void InfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info)
{
    camera_info_ = (*camera_info);
    has_camera_info_ = true;
}

// Callback for image data
void ImageCallback(const sensor_msgs::ImageConstPtr& msg )
{
    if(!has_camera_info_){
        ROS_WARN("No Camera Info Received Yet");
        return;
    }

    // Get the image
    cv_bridge::CvImagePtr subscribed_ptr;
    try
    {
        subscribed_ptr = cv_bridge::toCvCopy(msg, "mono8");
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat subscribed_gray = subscribed_ptr->image;
    cv::Mat tmp;
    cv::Point2d opticalCenter(0.5*subscribed_gray.rows,
                              0.5*subscribed_gray.cols);
    TagDetectionArray detections;
    detector_->process(subscribed_gray, opticalCenter, detections);
    visualization_msgs::MarkerArray marker_transforms;

    if(viewer_)
    {
        subscribed_gray = family_->superimposeDetections(subscribed_gray,
                                                         detections);
    }
    for(unsigned int i = 0; i < detections.size(); ++i)
    {
        Eigen::Matrix4d pose = GetDetectionTransform(detections[i]);

        // Get this info from earlier code, don't extract it again
        Eigen::Matrix3d R = pose.block<3,3>(0,0);
        Eigen::Quaternion<double> q(R);

        visualization_msgs::Marker marker_transform;
        marker_transform.header.frame_id = frame_;
        marker_transform.header.stamp = ros::Time::now();
        stringstream convert;
        convert << "tag" << detections[i].id;
        marker_transform.ns = convert.str().c_str();
        marker_transform.id = detections[i].id;
        //marker_transform.type = visualization_msgs::Marker::ARROW;
        marker_transform.type = visualization_msgs::Marker::CUBE;
        marker_transform.action = visualization_msgs::Marker::ADD;
        marker_transform.pose.position.x = pose(0,3);
        marker_transform.pose.position.y = pose(1,3);
        marker_transform.pose.position.z = pose(2,3);
        marker_transform.pose.orientation.x = q.x();
        marker_transform.pose.orientation.y = q.y();
        marker_transform.pose.orientation.z = q.z();
        marker_transform.pose.orientation.w = q.w();

    	double tag_size = GetTagSize(detections[i].id);

        marker_transform.scale.x = tag_size;
        marker_transform.scale.y = tag_size;
        marker_transform.scale.z = 0.01 * tag_size;
        /*
        marker_transform.scale.x = tag_size;
        marker_transform.scale.y = tag_size*10;
        marker_transform.scale.z = tag_size*0.5;
        */
        marker_transform.color.r = 1.0;
        marker_transform.color.g = 0.0;
        marker_transform.color.b = 1.0;
        marker_transform.color.a = 1.0;
        marker_transforms.markers.push_back(marker_transform);
    }
    marker_publisher_.publish(marker_transforms);

    if(viewer_)
    {
        cv::imshow("AprilTags", subscribed_gray);
    }
}

void ConnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers();
    ROS_DEBUG("Subscription detected! (%d subscribers)", subscribers);

    if(subscribers && !running_)
    {
        ROS_DEBUG("New Subscribers, Connecting to Input Image Topic.");
        ros::TransportHints ros_transport_hints(ros::TransportHints().tcpNoDelay());
        image_transport::TransportHints image_transport_hint(image_transport::TransportHints(
                                "raw", ros_transport_hints, (*node_),
                                "image_transport"));

        image_subscriber = (*image_).subscribe(
                DEFAULT_IMAGE_TOPIC, 1, &ImageCallback,
                image_transport_hint);
        info_subscriber = (*node_).subscribe(
                DEFAULT_CAMERA_INFO_TOPIC, 10, &InfoCallback);
        running_ = true;
    }
}

void DisconnectHandler()
{
}

void DisconnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers();
    ROS_DEBUG("Unsubscription detected! (%d subscribers)", subscribers);

    if(!subscribers && running_)
    {
        ROS_DEBUG("No Subscribers, Disconnecting from Input Image Topic.");
		image_subscriber.shutdown();
		info_subscriber.shutdown();
		running_ = false;
    }
}

void GetParameterValues()
{
    // Load node-wide configuration values.
    node_->param("viewer", viewer_, 0);
    node_->param("tag_family", tag_family_name_, DEFAULT_TAG_FAMILY);
    node_->param("default_tag_size", default_tag_size_, DEFAULT_TAG_SIZE);
    node_->param("tf_frame", frame_, DEFAULT_TF_FRAME);

    // Load tag specific configuration values.
    XmlRpc::XmlRpcValue tag_data;
    node_->param("tag_data", tag_data, tag_data);

    // Iterate through each tag in the configuration.
    XmlRpc::XmlRpcValue::ValueStruct::iterator it;
    for (it = tag_data.begin(); it != tag_data.end(); ++it)
    {
        // Retrieve the settings for the next tag.
        int tag_id = boost::lexical_cast<int>(it->first);
        XmlRpc::XmlRpcValue tag_values = it->second;

        // Load all the settings for this tag.
        if (tag_values.hasMember("size"))
        {
            tag_sizes_[tag_id] = static_cast<double>(tag_values["size"]);
            ROS_DEBUG("Setting tag%d to size %f m.", tag_id, tag_sizes_[tag_id]);
        }
    }
}

void SetupPublisher()
{
    ros::SubscriberStatusCallback connect_callback = &ConnectCallback;
    ros::SubscriberStatusCallback disconnect_callback = &DisconnectCallback;

    // Publisher
    marker_publisher_ = node_->advertise<visualization_msgs::MarkerArray>(
            DEFAULT_MARKER_TOPIC, 1, connect_callback,
            disconnect_callback);
}

void InitializeTags()
{
    tag_params.newQuadAlgorithm = 1;
    family_ = new TagFamily(tag_family_name_);
    detector_ = new TagDetector(*family_, tag_params);
}

void InitializeROSNode(int argc, char **argv)
{
    ros::init(argc, argv, "apriltags");
    node_ =  boost::make_shared<ros::NodeHandle>("~");
    image_ = boost::make_shared<image_transport::ImageTransport>(*node_);
}

int main(int argc, char **argv)
{
    InitializeROSNode(argc,argv);
    GetParameterValues();
    SetupPublisher();
    InitializeTags();

    if(viewer_){
        cvNamedWindow("AprilTags");
        cvStartWindowThread();
    }

    ROS_INFO("AprilTags node started.");
    running_ = false;
    has_camera_info_ = false;
    ros::spin();
    ROS_INFO("AprilTags node stopped.");

    //Destroying Stuff
    cvDestroyWindow("AprilTags");
    delete detector_;
    delete family_;

    return EXIT_SUCCESS;
}
<|endoftext|>


<commit_before>#include "EweApp.h"
#include "Moose.h"
#include "AppFactory.h"
#include "ModulesApp.h"

#include "SecondOrderImplicitEulerWithDensity.h"
#include "SecondDerivativeNewmark.h"
#include "FirstDerivativeNewmark.h"
#include "KineticEnergyNewmarkAux.h"
#include "KineticEnergyAux.h"
#include "VolumeNearestNodeDistanceAux.h"
#include "CardiacThicknessParameterAux.h"
#include "NeighborElementAverageAux.h"
#include "NewmarkMaterial.h"
#include "CardiacStressDivergence.h"
#include "CardiacKirchhoffStressDivergence.h"
\#include "CardiacLinearOrthotropicMaterial.h"
#include "CardiacNash2000Material.h"
#include "CardiacHolzapfel2009Material.h"
#include "CardiacCostaMaterial.h"
#include "CardiacLinearMaterial.h"
#include "DisplacementAux.h"
#include "AnisotropicGradientShift.h"
#include "CardiacMaterialVolumeRatio.h"
#include "CardiacVolumeRatio.h"
#include "CardiacKirchhoffIncompressibilityPenalty.h"
#include "CardiacKirchhoffIncompressibilityLagrangeMultiplier.h"

#include "ActiveTensionODE.h"

#include "Electrocardio.h"
#include "ElectrocardioForcing.h"
#include "ElectrocardioTimeDerivative.h"
#include "ElectrocardioDiffusion.h"
#include "ElectrocardioConductivity.h"
#include "ElectrocardioIC.h"
#include "CardiacFibresMaterial.h"
#include "PiecewiseFunction.h"

template<>
InputParameters validParams<EweApp>()
{
  InputParameters params = validParams<MooseApp>();
  return params;
}

EweApp::EweApp(const std::string & name, InputParameters parameters) :
    MooseApp(name, parameters)
{
  srand(processor_id());

  Moose::registerObjects(_factory);
  ModulesApp::registerObjects(_factory);
  EweApp::registerObjects(_factory);

  Moose::associateSyntax(_syntax, _action_factory);
  ModulesApp::associateSyntax(_syntax, _action_factory);
  EweApp::associateSyntax(_syntax, _action_factory);
}

EweApp::~EweApp()
{
}

void
EweApp::registerApps()
{
  registerApp(EweApp);
}

void
EweApp::registerObjects(Factory & factory)
{
  // Register any custom objects you have built on the MOOSE Framework
  registerKernel(SecondOrderImplicitEulerWithDensity);
  registerKernel(SecondDerivativeNewmark);
  registerKernel(FirstDerivativeNewmark);
  registerKernel(CardiacStressDivergence);
  registerKernel(CardiacKirchhoffStressDivergence);
  registerKernel(ElectrocardioForcing);
  registerKernel(AnisotropicGradientShift);
  registerKernel(CardiacKirchhoffIncompressibilityPenalty);
  registerKernel(CardiacKirchhoffIncompressibilityLagrangeMultiplier);

  registerMaterial(NewmarkMaterial);
  registerMaterial(Electrocardio);
  registerMaterial(ElectrocardioConductivity);
  registerMaterial(CardiacLinearOrthotropicMaterial);
  registerMaterial(CardiacNash2000Material);
  registerMaterial(CardiacHolzapfel2009Material);
  registerMaterial(CardiacCostaMaterial);
  registerMaterial(CardiacLinearMaterial);
  registerMaterial(CardiacFibresMaterial);

  registerMaterial(ActiveTensionODE);

  registerKernel(ElectrocardioTimeDerivative);
  registerKernel(ElectrocardioDiffusion);

  registerInitialCondition(ElectrocardioIC);

  registerAux(KineticEnergyNewmarkAux);
  registerAux(KineticEnergyAux);
  registerAux(DisplacementAux);
  registerAux(VolumeNearestNodeDistanceAux);
  registerAux(CardiacThicknessParameterAux);
  registerAux(NeighborElementAverageAux);

  registerPostprocessor(CardiacMaterialVolumeRatioPostprocessor);
  registerPostprocessor(CardiacVolumeRatioPostprocessor);

  registerFunction(PiecewiseFunction);
}

void
EweApp::associateSyntax(Syntax & /*syntax*/, ActionFactory & /*action_factory*/)
{
}
<commit_msg>fixed a typos, sorry<commit_after>#include "EweApp.h"
#include "Moose.h"
#include "AppFactory.h"
#include "ModulesApp.h"

#include "SecondOrderImplicitEulerWithDensity.h"
#include "SecondDerivativeNewmark.h"
#include "FirstDerivativeNewmark.h"
#include "KineticEnergyNewmarkAux.h"
#include "KineticEnergyAux.h"
#include "VolumeNearestNodeDistanceAux.h"
#include "CardiacThicknessParameterAux.h"
#include "NeighborElementAverageAux.h"
#include "NewmarkMaterial.h"
#include "CardiacStressDivergence.h"
#include "CardiacKirchhoffStressDivergence.h"
#include "CardiacLinearOrthotropicMaterial.h"
#include "CardiacNash2000Material.h"
#include "CardiacHolzapfel2009Material.h"
#include "CardiacCostaMaterial.h"
#include "CardiacLinearMaterial.h"
#include "DisplacementAux.h"
#include "AnisotropicGradientShift.h"
#include "CardiacMaterialVolumeRatio.h"
#include "CardiacVolumeRatio.h"
#include "CardiacKirchhoffIncompressibilityPenalty.h"
#include "CardiacKirchhoffIncompressibilityLagrangeMultiplier.h"

#include "ActiveTensionODE.h"

#include "Electrocardio.h"
#include "ElectrocardioForcing.h"
#include "ElectrocardioTimeDerivative.h"
#include "ElectrocardioDiffusion.h"
#include "ElectrocardioConductivity.h"
#include "ElectrocardioIC.h"
#include "CardiacFibresMaterial.h"
#include "PiecewiseFunction.h"

template<>
InputParameters validParams<EweApp>()
{
  InputParameters params = validParams<MooseApp>();
  return params;
}

EweApp::EweApp(const std::string & name, InputParameters parameters) :
    MooseApp(name, parameters)
{
  srand(processor_id());

  Moose::registerObjects(_factory);
  ModulesApp::registerObjects(_factory);
  EweApp::registerObjects(_factory);

  Moose::associateSyntax(_syntax, _action_factory);
  ModulesApp::associateSyntax(_syntax, _action_factory);
  EweApp::associateSyntax(_syntax, _action_factory);
}

EweApp::~EweApp()
{
}

void
EweApp::registerApps()
{
  registerApp(EweApp);
}

void
EweApp::registerObjects(Factory & factory)
{
  // Register any custom objects you have built on the MOOSE Framework
  registerKernel(SecondOrderImplicitEulerWithDensity);
  registerKernel(SecondDerivativeNewmark);
  registerKernel(FirstDerivativeNewmark);
  registerKernel(CardiacStressDivergence);
  registerKernel(CardiacKirchhoffStressDivergence);
  registerKernel(ElectrocardioForcing);
  registerKernel(AnisotropicGradientShift);
  registerKernel(CardiacKirchhoffIncompressibilityPenalty);
  registerKernel(CardiacKirchhoffIncompressibilityLagrangeMultiplier);

  registerMaterial(NewmarkMaterial);
  registerMaterial(Electrocardio);
  registerMaterial(ElectrocardioConductivity);
  registerMaterial(CardiacLinearOrthotropicMaterial);
  registerMaterial(CardiacNash2000Material);
  registerMaterial(CardiacHolzapfel2009Material);
  registerMaterial(CardiacCostaMaterial);
  registerMaterial(CardiacLinearMaterial);
  registerMaterial(CardiacFibresMaterial);

  registerMaterial(ActiveTensionODE);

  registerKernel(ElectrocardioTimeDerivative);
  registerKernel(ElectrocardioDiffusion);

  registerInitialCondition(ElectrocardioIC);

  registerAux(KineticEnergyNewmarkAux);
  registerAux(KineticEnergyAux);
  registerAux(DisplacementAux);
  registerAux(VolumeNearestNodeDistanceAux);
  registerAux(CardiacThicknessParameterAux);
  registerAux(NeighborElementAverageAux);

  registerPostprocessor(CardiacMaterialVolumeRatioPostprocessor);
  registerPostprocessor(CardiacVolumeRatioPostprocessor);

  registerFunction(PiecewiseFunction);
}

void
EweApp::associateSyntax(Syntax & /*syntax*/, ActionFactory & /*action_factory*/)
{
}
<|endoftext|>


<commit_before>
#include <topic_tools/shape_shifter.h>
#include <boost/algorithm/string.hpp>
#include <boost/serialization/serialization.hpp>
#include <boost/utility/string_ref.hpp>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/JointState.h>
#include <tf/tfMessage.h>
#include <sstream>
#include <iostream>
#include <chrono>
#include <ros_type_introspection/renamer.hpp>

using namespace ros::message_traits;
using namespace RosIntrospection;

std::vector<SubstitutionRule> Rules()
{
    std::vector<SubstitutionRule> rules;
    rules.push_back( SubstitutionRule(".position[#]",
                                      ".name[#]",
                                      ".#.position") );

    rules.push_back( SubstitutionRule(".velocity[#]",
                                      ".name[#]",
                                      ".#.velocity") );

    rules.push_back( SubstitutionRule(".effort[#]",
                                      ".name[#]",
                                      ".#.effort") );

    rules.push_back( SubstitutionRule(".transforms[#].transform",
                                      ".transforms[#].header.frame_id",
                                      ".transform.#") );

    rules.push_back( SubstitutionRule(".transforms[#].header",
                                      ".transforms[#].header.frame_id",
                                      ".transform.#.header") );
    return rules;
}

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

    ROSTypeList type_map =  buildROSTypeMapFromDefinition(
                DataType<tf::tfMessage >::value(),
                Definition<tf::tfMessage>::value() );

    std::cout << "------------------------------"  << std::endl;
    std::cout << type_map << std::endl;

    tf::tfMessage tf_msg;

    tf_msg.transforms.resize(6);

    const char* suffix[6] = { "_A", "_B", "_C", "_D" , "_E", "_F"};

    for (int i=0; i< tf_msg.transforms.size() ; i++)
    {
        tf_msg.transforms[i].header.seq = 100+i;
        tf_msg.transforms[i].header.stamp.sec = 1234;
        tf_msg.transforms[i].header.frame_id = std::string("frame").append(suffix[i]);

        tf_msg.transforms[i].child_frame_id = std::string("child").append(suffix[i]);
        tf_msg.transforms[i].transform.translation.x = 10 +i;
        tf_msg.transforms[i].transform.translation.y = 20 +i;
        tf_msg.transforms[i].transform.translation.z = 30 +i;

        tf_msg.transforms[i].transform.rotation.x = 40 +i;
        tf_msg.transforms[i].transform.rotation.y = 50 +i;
        tf_msg.transforms[i].transform.rotation.z = 60 +i;
        tf_msg.transforms[i].transform.rotation.w = 70 +i;
    }

    std::vector<uint8_t> buffer(64*1024);

    auto start = std::chrono::high_resolution_clock::now();

    ros::serialization::OStream stream(buffer.data(), buffer.size());
    ros::serialization::Serializer<tf::tfMessage>::write(stream, tf_msg);
    ROSType main_type (DataType<tf::tfMessage >::value());

    for (int i=0; i<100000;i++)
    {
        uint8_t* buffer_ptr = buffer.data();

        ROSTypeFlat flat_container = buildRosFlatType(type_map,main_type, "msgTransform", &buffer_ptr);
      //  applyNameTransform( Rules(), &flat_container );
    }

    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = end - start;

    std::cout << "time elapsed: " << std::chrono::duration_cast< std::chrono::milliseconds>( elapsed ).count()  <<   std::endl;

    return 0;
}

<commit_msg>backup<commit_after>
#include <topic_tools/shape_shifter.h>
#include <boost/algorithm/string.hpp>
#include <boost/serialization/serialization.hpp>
#include <boost/utility/string_ref.hpp>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/JointState.h>
#include <tf/tfMessage.h>
#include <sstream>
#include <iostream>
#include <chrono>
#include <ros_type_introspection/renamer.hpp>

using namespace ros::message_traits;
using namespace RosIntrospection;

std::vector<SubstitutionRule> Rules()
{
    std::vector<SubstitutionRule> rules;
   /* rules.push_back( SubstitutionRule(".position[#]",
                                      ".name[#]",
                                      ".#.position") );

    rules.push_back( SubstitutionRule(".velocity[#]",
                                      ".name[#]",
                                      ".#.velocity") );

    rules.push_back( SubstitutionRule(".effort[#]",
                                      ".name[#]",
                                      ".#.effort") );
*/
    rules.push_back( SubstitutionRule(".transforms[#].transform",
                                      ".transforms[#].header.frame_id",
                                      ".transform.#") );

   /* rules.push_back( SubstitutionRule(".transforms[#].header",
                                      ".transforms[#].header.frame_id",
                                      ".transform.#.header"));*/
    return rules;
}

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

    ROSTypeList type_map =  buildROSTypeMapFromDefinition(
                DataType<tf::tfMessage >::value(),
                Definition<tf::tfMessage>::value() );

    std::cout << "------------------------------"  << std::endl;
    std::cout << type_map << std::endl;

    tf::tfMessage tf_msg;

    tf_msg.transforms.resize(6);

    const char* suffix[6] = { "_A", "_B", "_C", "_D" , "_E", "_F"};

    for (int i=0; i< tf_msg.transforms.size() ; i++)
    {
        tf_msg.transforms[i].header.seq = 100+i;
        tf_msg.transforms[i].header.stamp.sec = 1234;
        tf_msg.transforms[i].header.frame_id = std::string("frame").append(suffix[i]);

        tf_msg.transforms[i].child_frame_id = std::string("child").append(suffix[i]);
        tf_msg.transforms[i].transform.translation.x = 10 +i;
        tf_msg.transforms[i].transform.translation.y = 20 +i;
        tf_msg.transforms[i].transform.translation.z = 30 +i;

        tf_msg.transforms[i].transform.rotation.x = 40 +i;
        tf_msg.transforms[i].transform.rotation.y = 50 +i;
        tf_msg.transforms[i].transform.rotation.z = 60 +i;
        tf_msg.transforms[i].transform.rotation.w = 70 +i;
    }

    std::vector<uint8_t> buffer(64*1024);

    auto start = std::chrono::high_resolution_clock::now();

    ros::serialization::OStream stream(buffer.data(), buffer.size());
    ros::serialization::Serializer<tf::tfMessage>::write(stream, tf_msg);
    ROSType main_type (DataType<tf::tfMessage >::value());

    for (int i=0; i<100*1000;i++)
    {
        uint8_t* buffer_ptr = buffer.data();

        ROSTypeFlat flat_container = buildRosFlatType(type_map,main_type, "msgTransform", &buffer_ptr);
        applyNameTransform( Rules(), &flat_container );
    }

    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = end - start;

    std::cout << "time elapsed: " << std::chrono::duration_cast< std::chrono::milliseconds>( elapsed ).count()  <<   std::endl;

    return 0;
}

<|endoftext|>


<commit_before>/*******************************************************************************
 * src/benchmark.cpp
 *
 * Copyright (C) 2017 Florian Kurpicz <florian.kurpicz@tu-dortmund.de>
 *
 * All rights reserved. Published under the BSD-2 license in the LICENSE file.
 ******************************************************************************/

#include <tclap/CmdLine.h>
#include <vector>

#include "benchmark/algorithm.hpp"
#include "benchmark/file_util.hpp"

auto filter_parallel(bool only_parallel, bool is_parallel) {
  return (!only_parallel || is_parallel);
}

auto filter_sequential(bool sequential, bool is_parallel) {
  return (!sequential || !is_parallel);
}

auto filter_wavelet_type(bool is_tree, bool no_trees, bool no_matrices) {
  return (is_tree ? !no_trees : !no_matrices);
}

int32_t main(int32_t argc, char const* argv[]) {
  TCLAP::CmdLine cmd("Benchmark for wavelet tree (and matrix) construction",
    ' ', "0.2");

  TCLAP::SwitchArg list_all_algorithms("l", "list",
    "Print the name and description of all registered algorithms", false);
  cmd.add(list_all_algorithms);
  TCLAP::MultiArg<std::string> file_path_arg("f", "file",
    "Path to the text file.", false, "string");
  cmd.add(file_path_arg);
  TCLAP::ValueArg<std::string> filter_arg("n", "name",
    "Runs all algorithms that contain the <name> in their name", false, "",
    "string");
  cmd.add(filter_arg);
  TCLAP::ValueArg<int32_t> word_width_arg("b", "byte",
    "Bytes per char in the input text.", false, 1, "uint8_t");
  cmd.add(word_width_arg);
  TCLAP::ValueArg<int32_t> nr_runs_arg("r", "runs",
    "Number of repetitions of the construction algorithm.", false, 5, "int32_t");
  cmd.add(nr_runs_arg);
  TCLAP::SwitchArg run_only_parallel_arg("p", "parallel",
    "Run only parallel construction algorithms.", false);
  cmd.add(run_only_parallel_arg);
  TCLAP::SwitchArg run_only_sequential_arg("s", "sequential",
    "Run only sequential construction algorithms.", false);
  cmd.add(run_only_sequential_arg);
  TCLAP::SwitchArg no_trees_arg("m", "no_trees",
    "Skip all wavelet trees construction algorithms.", false);
  cmd.add(no_trees_arg);
  TCLAP::SwitchArg no_matrices_arg("t", "no_matrices",
    "Skip all wavelet matrices construction algorithms.", false);
  cmd.add(no_matrices_arg);
  cmd.parse( argc, argv );

  auto& algo_list = algorithm_list::get_algorithm_list();
  if (list_all_algorithms.getValue()) {
    for (const auto& a : algo_list) {
      a->print_info();
    }
    return 0;
  }

  const std::vector<std::string> file_paths = file_path_arg.getValue();
  std::string filter = filter_arg.getValue();
  const int32_t word_width = word_width_arg.getValue();
  const int32_t nr_runs = nr_runs_arg.getValue();
  const bool run_only_parallel = run_only_parallel_arg.getValue();
  const bool run_only_sequential = run_only_sequential_arg.getValue();
  const bool no_trees = no_trees_arg.getValue();
  const bool no_matrices = no_matrices_arg.getValue();

  for (const auto& path : file_paths) {
    auto text = file_to_vector<1>(path);
    std::cout << std::endl << "Text: " << path << std::endl;
    uint64_t levels = reduce_alphabet(text);
    for (const auto& a : algo_list) {
      if (filter == "" || (a->name().find(filter) != std::string::npos)) {
        if (a->word_width() == word_width) {
          if (filter_parallel(run_only_parallel, a->is_parallel())) {
            if (filter_sequential(run_only_sequential, a->is_parallel())) {
              if (filter_wavelet_type(a->is_tree(), no_trees, no_matrices)) {
                a->print_info();
                std::cout << a->median_time(&text, text.size(), levels, nr_runs)
                          << std::endl;
              }
            }
          }
        }
      }
    }
  }
  return 0;
}

/******************************************************************************/
<commit_msg>Add benchmark support for larger alphabets<commit_after>/*******************************************************************************
 * src/benchmark.cpp
 *
 * Copyright (C) 2017 Florian Kurpicz <florian.kurpicz@tu-dortmund.de>
 *
 * All rights reserved. Published under the BSD-2 license in the LICENSE file.
 ******************************************************************************/

#include <tclap/CmdLine.h>
#include <vector>

#include "benchmark/algorithm.hpp"
#include "benchmark/file_util.hpp"

auto filter_parallel(bool only_parallel, bool is_parallel) {
  return (!only_parallel || is_parallel);
}

auto filter_sequential(bool sequential, bool is_parallel) {
  return (!sequential || !is_parallel);
}

auto filter_wavelet_type(bool is_tree, bool no_trees, bool no_matrices) {
  return (is_tree ? !no_trees : !no_matrices);
}

int32_t main(int32_t argc, char const* argv[]) {
  TCLAP::CmdLine cmd("Benchmark for wavelet tree (and matrix) construction",
    ' ', "0.2");

  TCLAP::SwitchArg list_all_algorithms("l", "list",
    "Print the name and description of all registered algorithms", false);
  cmd.add(list_all_algorithms);
  TCLAP::MultiArg<std::string> file_path_arg("f", "file",
    "Path to the text file.", false, "string");
  cmd.add(file_path_arg);
  TCLAP::ValueArg<std::string> filter_arg("n", "name",
    "Runs all algorithms that contain the <name> in their name", false, "",
    "string");
  cmd.add(filter_arg);
  TCLAP::ValueArg<int32_t> word_width_arg("b", "byte",
    "Bytes per char in the input text.", false, 1, "uint8_t");
  cmd.add(word_width_arg);
  TCLAP::ValueArg<int32_t> nr_runs_arg("r", "runs",
    "Number of repetitions of the construction algorithm.",
    false, 5, "int32_t");
  cmd.add(nr_runs_arg);
  TCLAP::SwitchArg run_only_parallel_arg("p", "parallel",
    "Run only parallel construction algorithms.", false);
  cmd.add(run_only_parallel_arg);
  TCLAP::SwitchArg run_only_sequential_arg("s", "sequential",
    "Run only sequential construction algorithms.", false);
  cmd.add(run_only_sequential_arg);
  TCLAP::SwitchArg no_trees_arg("m", "no_trees",
    "Skip all wavelet trees construction algorithms.", false);
  cmd.add(no_trees_arg);
  TCLAP::SwitchArg no_matrices_arg("t", "no_matrices",
    "Skip all wavelet matrices construction algorithms.", false);
  cmd.add(no_matrices_arg);
  cmd.parse( argc, argv );

  auto& algo_list = algorithm_list::get_algorithm_list();
  if (list_all_algorithms.getValue()) {
    for (const auto& a : algo_list) {
      a->print_info();
    }
    return 0;
  }

  const std::vector<std::string> file_paths = file_path_arg.getValue();
  std::string filter = filter_arg.getValue();
  const int32_t word_width = word_width_arg.getValue();
  const int32_t nr_runs = nr_runs_arg.getValue();
  const bool run_only_parallel = run_only_parallel_arg.getValue();
  const bool run_only_sequential = run_only_sequential_arg.getValue();
  const bool no_trees = no_trees_arg.getValue();
  const bool no_matrices = no_matrices_arg.getValue();

  for (const auto& path : file_paths) {
    std::cout << std::endl << "Text: " << path << std::endl;
    void* txt_prt = nullptr;
    uint64_t text_size = 0;
    uint64_t levels = 0;
    std::vector<uint8_t> text_uint8;
    std::vector<uint16_t> text_uint16;
    std::vector<uint32_t> text_uint32;
    std::vector<uint64_t> text_uint64;
    if (word_width == 1) {
      text_uint8 = file_to_vector<1>(path);
      text_size = text_uint8.size();
      levels = reduce_alphabet(text_uint8);
      txt_prt = &text_uint8;
    } else if (word_width == 2) {
      text_uint16 = file_to_vector<2>(path);
      text_size = text_uint16.size();
      levels = reduce_alphabet(text_uint16);
      txt_prt = &text_uint16;
    } else if (word_width == 4) {
      text_uint32 = file_to_vector<4>(path);
      text_size = text_uint32.size();
      levels = reduce_alphabet(text_uint32);
      txt_prt = &text_uint32;
    } else if (word_width == 8) {
      text_uint64 = file_to_vector<8>(path);
      text_size = text_uint64.size();
      levels = reduce_alphabet(text_uint64);
      txt_prt = &text_uint64;
    } else {
      std::cerr << "You entered an invalid number of bytes per character "
                   "(parameter 'b')." << std::endl;
      return -1;
    }
    for (const auto& a : algo_list) {
      if (filter == "" || (a->name().find(filter) != std::string::npos)) {
        if (a->word_width() == word_width) {
          if (filter_parallel(run_only_parallel, a->is_parallel())) {
            if (filter_sequential(run_only_sequential, a->is_parallel())) {
              if (filter_wavelet_type(a->is_tree(), no_trees, no_matrices)) {
                a->print_info();
                std::cout << a->median_time(txt_prt, text_size, levels, nr_runs)
                          << std::endl;
              }
            }
          }
        }
      }
    }
  }
  return 0;
}

/******************************************************************************/
<|endoftext|>


<commit_before>/* This is free and unencumbered software released into the public domain. */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "error.h"

#include <cassert> /* for assert() */
#include <cerrno>  /* for errno */
#include <cstdarg> /* for va_list, va_*(), vsnprintf() */
#include <cstdio>  /* for std::fprintf() */
#include <cstdlib> /* for std::abort() */
#include <cstring> /* for std::strerror() */

const size_t cpr_error_sizeof = sizeof(cpr_error_t);

cpr_error_hook_t cpr_error_hook = nullptr;

/**
 * A thread-local buffer large enough to hold typical formatted error
 * messages. Messages larger than the capacity of the buffer are truncated
 * at `sizeof(error_buffer)-1`. The buffer is always NUL-terminated.
 */
#if __has_feature(cxx_thread_local)
static thread_local char error_buffer[4096];
#else
static __thread char error_buffer[4096];
#endif

static const char* error_types[] = {
  nullptr,   /* CPR_ERROR_TYPE_UNDEFINED */
  "Logic",   /* CPR_ERROR_TYPE_LOGIC     */
  "Runtime", /* CPR_ERROR_TYPE_RUNTIME   */
  "Fatal",   /* CPR_ERROR_TYPE_FATAL     */
  nullptr
};

static const size_t error_types_count =
  (sizeof(error_types) / sizeof(error_types[0])) - 1;

static void
cpr_error_print(const cpr_error_t* const error,
                FILE* const stream) {
  if (!error || !error->code) return;

  assert(error->type >= CPR_ERROR_TYPE_UNDEFINED &&
         error->type <= CPR_ERROR_TYPE_FATAL);
  const char* const error_type = (error->type < error_types_count) ?
    error_types[error->type] : nullptr;

  const char* const error_message = error->message ?
    error->message : std::strerror(error->code);

  if (!error_type) {
    std::fprintf(stream, "Error: ");
  }
  else {
    std::fprintf(stream, "%s error: ", error_type);
  }

  std::fprintf(stream, "%s (%d)\n", error_message, error->code);
}

bool
cpr_abort_with_error(const cpr_error_t* const error) {
  if (error) {
    cpr_error_print(error, stderr);
    std::fprintf(stderr, "Aborted in function %s() on line %u of %s.\n",
      error->function, error->line, error->file);
  }

  std::abort();

  return false; /* never reached */
}

void
_cpr_abort(const char* const function,
           const char* const file,
           const unsigned int line) {
  const cpr_error_t error = {
    /*.type     =*/ CPR_ERROR_TYPE_UNDEFINED,
    /*.code     =*/ 0,
    /*.message  =*/ nullptr,
    /*.function =*/ function,
    /*.file     =*/ file,
    /*.line     =*/ line,
  };

  (void)cpr_abort_with_error(&error);
}

bool
_cpr_error(const char* const function,
           const char* const file,
           const unsigned int line,
           const cpr_error_type_t error_type,
           const cpr_error_code_t error_code,
           const char* const error_format, ...) {
  errno = error_code;

  if (!cpr_error_hook) {
    return false; /* don't retry */
  }

  if (error_format) {
    va_list args;
    va_start(args, error_format);
    (void)vsnprintf(error_buffer, sizeof(error_buffer), error_format, args);
    va_end(args);
  }

  const cpr_error_t error = {
    /*.type     =*/ error_type,
    /*.code     =*/ error_code,
    /*.message  =*/ error_format ? error_buffer : nullptr,
    /*.function =*/ function,
    /*.file     =*/ file,
    /*.line     =*/ line,
  };

  return cpr_error_hook(&error);
}
<commit_msg>Added a cast to defeat Clang's too-clever-by-half "is always true" warning emitted with -Wtautological-constant-out-of-range-compare.<commit_after>/* This is free and unencumbered software released into the public domain. */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "error.h"

#include <cassert> /* for assert() */
#include <cerrno>  /* for errno */
#include <cstdarg> /* for va_list, va_*(), vsnprintf() */
#include <cstdio>  /* for std::fprintf() */
#include <cstdlib> /* for std::abort() */
#include <cstring> /* for std::strerror() */

const std::size_t cpr_error_sizeof = sizeof(cpr_error_t);

cpr_error_hook_t cpr_error_hook = nullptr;

/**
 * A thread-local buffer large enough to hold typical formatted error
 * messages. Messages larger than the capacity of the buffer are truncated
 * at `sizeof(error_buffer)-1`. The buffer is always NUL-terminated.
 */
#if __has_feature(cxx_thread_local)
static thread_local char error_buffer[4096];
#else
static __thread char error_buffer[4096];
#endif

static const char* error_types[] = {
  nullptr,   /* CPR_ERROR_TYPE_UNDEFINED */
  "Logic",   /* CPR_ERROR_TYPE_LOGIC     */
  "Runtime", /* CPR_ERROR_TYPE_RUNTIME   */
  "Fatal",   /* CPR_ERROR_TYPE_FATAL     */
  nullptr
};

static const std::size_t error_types_count =
  (sizeof(error_types) / sizeof(error_types[0])) - 1;

static void
cpr_error_print(const cpr_error_t* const error,
                FILE* const stream) {
  if (!error || !error->code) return;

  assert(error->type >= CPR_ERROR_TYPE_UNDEFINED &&
         error->type <= CPR_ERROR_TYPE_FATAL);
  const char* const error_type = (static_cast<std::size_t>(error->type) < error_types_count) ?
    error_types[error->type] : nullptr;

  const char* const error_message = error->message ?
    error->message : std::strerror(error->code);

  if (!error_type) {
    std::fprintf(stream, "Error: ");
  }
  else {
    std::fprintf(stream, "%s error: ", error_type);
  }

  std::fprintf(stream, "%s (%d)\n", error_message, error->code);
}

bool
cpr_abort_with_error(const cpr_error_t* const error) {
  if (error) {
    cpr_error_print(error, stderr);
    std::fprintf(stderr, "Aborted in function %s() on line %u of %s.\n",
      error->function, error->line, error->file);
  }

  std::abort();

  return false; /* never reached */
}

void
_cpr_abort(const char* const function,
           const char* const file,
           const unsigned int line) {
  const cpr_error_t error = {
    /*.type     =*/ CPR_ERROR_TYPE_UNDEFINED,
    /*.code     =*/ 0,
    /*.message  =*/ nullptr,
    /*.function =*/ function,
    /*.file     =*/ file,
    /*.line     =*/ line,
  };

  (void)cpr_abort_with_error(&error);
}

bool
_cpr_error(const char* const function,
           const char* const file,
           const unsigned int line,
           const cpr_error_type_t error_type,
           const cpr_error_code_t error_code,
           const char* const error_format, ...) {
  errno = error_code;

  if (!cpr_error_hook) {
    return false; /* don't retry */
  }

  if (error_format) {
    va_list args;
    va_start(args, error_format);
    (void)vsnprintf(error_buffer, sizeof(error_buffer), error_format, args);
    va_end(args);
  }

  const cpr_error_t error = {
    /*.type     =*/ error_type,
    /*.code     =*/ error_code,
    /*.message  =*/ error_format ? error_buffer : nullptr,
    /*.function =*/ function,
    /*.file     =*/ file,
    /*.line     =*/ line,
  };

  return cpr_error_hook(&error);
}
<|endoftext|>


<commit_before>// dllmain.cpp : Defines the entry point for the DLL application.
#include <Windows.h>
#include "Structures.h"
#include "PatternScanner.h"

#define KEY_DOWN 0x100
#define KEY_UP 0x101
#define VK_LEFT 0x25
#define VK_RIGHT 0x27

#define WOOD 1
#define BOMB 2

DWORD WINAPI DLLStart(LPVOID param)
{
	//Get pointer that points to TMiniGameManager pointer
	TMiniGameManager* manager = **(TMiniGameManager***)(FindPattern((BYTE*)0x500000,
														0x200000,
														(BYTE*)("\xA1\x00\x00\x00\x00\x8B\x08\xFF\x51\x44\xA1\x00\x00\x00\x00\xE8\x00\x00\x00\x00\x33\xD2"), "x????xxxxxx????x????xx") + 1);

	MiniGame_1* game = manager->miniGameManagerList->getMiniGame();

	//Check if the pointer to MiniGame_1 is correct
	if (IsBadReadPtr(game, sizeof(MiniGame_1)))
		return -1;

	//Get the paddles data
	void* leftPaddleData = game->leftPaddle->data;
	void* rightPaddleData = game->rightPaddle->data;

	//Find hwnd of NosTale window
	HWND hwnd = FindWindowA("TNosTaleMainF", "NosTale");

	while (true)
	{
		//Chceck if minigame is on
		if (game->m_bIsOn)
		{

			//Paddle hitbox area(the place where we can hit) has 110 bytes (397-287).
			//If the wood is between byte 40 and byte 70 we get the "Nice" result, and as a result, the "combo x"
			//Really the front of the paddle is at the back of memory, so at offset 397 is the start of the paddle, and at 287 is the end of paddle
			//We iterate the last(really first) 70 bytes, because we don't want to lose wood, when it will be at the byte lower than 40, but we still prefer to get combo
			for (int i = 397; i != 327; --i) // 287 max
			{
				if (*(BYTE*)((DWORD)leftPaddleData + (DWORD)i) == WOOD)
				{
					PostMessage(hwnd, KEY_DOWN, VK_LEFT, 0);
					PostMessage(hwnd, KEY_UP, VK_LEFT, 0);
					Sleep(2);
					break;
				}

				if (*(BYTE*)((DWORD)rightPaddleData + (DWORD)i) == WOOD)
				{
					PostMessage(hwnd, KEY_DOWN, VK_RIGHT, 0);
					PostMessage(hwnd, KEY_UP, VK_RIGHT, 0);
					Sleep(2);
					break;
				}
			}
		}

		Sleep(1);
	}

	return 0;
}

BOOL APIENTRY DllMain( HMODULE hModule,
                       DWORD  ul_reason_for_call,
                       LPVOID lpReserved
					 )
{
	switch (ul_reason_for_call)
	{
	case DLL_PROCESS_ATTACH:
		CreateThread(0, 0, DLLStart, 0, 0, 0);
	case DLL_THREAD_ATTACH:
	case DLL_THREAD_DETACH:
	case DLL_PROCESS_DETACH:
		break;
	}
	return TRUE;
}

<commit_msg>visual code changes<commit_after>// dllmain.cpp : Defines the entry point for the DLL application.
#include <Windows.h>
#include "Structures.h"
#include "PatternScanner.h"

#define KEY_DOWN 0x100
#define KEY_UP 0x101
#define VK_LEFT 0x25
#define VK_RIGHT 0x27

#define WOOD 1
#define BOMB 2

DWORD WINAPI DLLStart(LPVOID param)
{
	//Get pointer that points to TMiniGameManager pointer
	TMiniGameManager* manager = **(TMiniGameManager***)(FindPattern((BYTE*)0x500000,
					0x200000,
					(BYTE*)("\xA1\x00\x00\x00\x00\x8B\x08\xFF\x51\x44\xA1\x00\x00\x00\x00\xE8\x00\x00\x00\x00\x33\xD2"), "x????xxxxxx????x????xx") + 1);

	MiniGame_1* game = manager->miniGameManagerList->getMiniGame();

	//Check if the pointer to MiniGame_1 is correct
	if (IsBadReadPtr(game, sizeof(MiniGame_1)))
		return -1;

	//Get the paddles data
	void* leftPaddleData = game->leftPaddle->data;
	void* rightPaddleData = game->rightPaddle->data;

	//Find hwnd of NosTale window
	HWND hwnd = FindWindowA("TNosTaleMainF", "NosTale");

	while (true)
	{
		//Chceck if minigame is on
		if (game->m_bIsOn)
		{

			//Paddle hitbox area(the place where we can hit) has 110 bytes (397-287).
			//If the wood is between byte 40 and byte 70 we get the "Nice" result, and as a result, the "combo x"
			//Really the front of the paddle is at the back of memory, so at offset 397 is the start of the paddle, and at 287 is the end of paddle
			//We iterate the last(really first) 70 bytes, because we don't want to lose wood, when it will be at the byte lower than 40, but we still prefer to get combo
			for (int i = 397; i != 327; --i) // 287 max
			{
				if (*(BYTE*)((DWORD)leftPaddleData + (DWORD)i) == WOOD)
				{
					PostMessage(hwnd, KEY_DOWN, VK_LEFT, 0);
					PostMessage(hwnd, KEY_UP, VK_LEFT, 0);
					Sleep(2);
					break;
				}

				if (*(BYTE*)((DWORD)rightPaddleData + (DWORD)i) == WOOD)
				{
					PostMessage(hwnd, KEY_DOWN, VK_RIGHT, 0);
					PostMessage(hwnd, KEY_UP, VK_RIGHT, 0);
					Sleep(2);
					break;
				}
			}
		}

		Sleep(1);
	}

	return 0;
}

BOOL APIENTRY DllMain( HMODULE hModule,
                       DWORD  ul_reason_for_call,
                       LPVOID lpReserved
					 )
{
	switch (ul_reason_for_call)
	{
	case DLL_PROCESS_ATTACH:
		CreateThread(0, 0, DLLStart, 0, 0, 0);
	case DLL_THREAD_ATTACH:
	case DLL_THREAD_DETACH:
	case DLL_PROCESS_DETACH:
		break;
	}
	return TRUE;
}

<|endoftext|>


<commit_before>#include "curl_fun.h"
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <string.h>

size_t WriteCallback(void* contents, size_t size, size_t nmemb, void* userp) {
  ((std::string*)userp)->append((char*)contents, size * nmemb);
  return size * nmemb;
}

json_t* getJsonFromUrl(Parameters& params, std::string url, std::string postFields) {
  curl_easy_setopt(params.curl, CURLOPT_URL, url.c_str());
  curl_easy_setopt(params.curl, CURLOPT_CONNECTTIMEOUT, 10L);
  curl_easy_setopt(params.curl, CURLOPT_TIMEOUT, 20L);
  if (!postFields.empty()) {
    curl_easy_setopt(params.curl, CURLOPT_POSTFIELDS, postFields.c_str());
  }
  curl_easy_setopt(params.curl, CURLOPT_WRITEFUNCTION, WriteCallback);
  std::string readBuffer;
  curl_easy_setopt(params.curl, CURLOPT_WRITEDATA, &readBuffer);
  curl_easy_setopt(params.curl, CURLOPT_DNS_CACHE_TIMEOUT, 3600);
  // WORKAROUND FOR BITFINEX: CHANGE TO GET REQUEST
  if (strcmp(url.c_str(), "https://api.bitfinex.com/v1/book/btcusd") == 0) {
    curl_easy_setopt(params.curl, CURLOPT_CUSTOMREQUEST, "GET");
  }
  CURLcode resCurl = curl_easy_perform(params.curl);
  *params.logFile << "Request finished" << std::endl;
  while (resCurl != CURLE_OK) {
    *params.logFile << "Error with cURL: " << curl_easy_strerror(resCurl) << std::endl;
    *params.logFile << "  URL: " << url << std::endl;
    *params.logFile << "  Retry in 2 sec..." << std::endl;
    sleep(2.0);
    readBuffer = "";
    curl_easy_setopt(params.curl, CURLOPT_DNS_CACHE_TIMEOUT, 0);
    resCurl = curl_easy_perform(params.curl);
  }
  json_t* root;
  json_error_t error;
  root = json_loads(readBuffer.c_str(), 0, &error);
  while (!root) {
    *params.logFile << "Error with JSON:\n" << error.text << std::endl;
    *params.logFile << "Buffer:\n" << readBuffer.c_str() << std::endl;
    *params.logFile << "Retrying..." << std::endl;
    sleep(2.0);
    readBuffer = "";
    resCurl = curl_easy_perform(params.curl);
    while (resCurl != CURLE_OK) {
      *params.logFile << "Error with cURL: " << curl_easy_strerror(resCurl) << std::endl;
      *params.logFile << "  URL: " << url << std::endl;
      *params.logFile << "  Retry in 2 sec..." << std::endl;
      sleep(2.0);
      readBuffer = "";
      resCurl = curl_easy_perform(params.curl);
    }
    root = json_loads(readBuffer.c_str(), 0, &error);
  }
  return root;
}

<commit_msg>Removed remaining print statement.<commit_after>#include "curl_fun.h"
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <string.h>

size_t WriteCallback(void* contents, size_t size, size_t nmemb, void* userp) {
  ((std::string*)userp)->append((char*)contents, size * nmemb);
  return size * nmemb;
}

json_t* getJsonFromUrl(Parameters& params, std::string url, std::string postFields) {
  curl_easy_setopt(params.curl, CURLOPT_URL, url.c_str());
  curl_easy_setopt(params.curl, CURLOPT_CONNECTTIMEOUT, 10L);
  curl_easy_setopt(params.curl, CURLOPT_TIMEOUT, 20L);
  if (!postFields.empty()) {
    curl_easy_setopt(params.curl, CURLOPT_POSTFIELDS, postFields.c_str());
  }
  curl_easy_setopt(params.curl, CURLOPT_WRITEFUNCTION, WriteCallback);
  std::string readBuffer;
  curl_easy_setopt(params.curl, CURLOPT_WRITEDATA, &readBuffer);
  curl_easy_setopt(params.curl, CURLOPT_DNS_CACHE_TIMEOUT, 3600);
  // WORKAROUND FOR BITFINEX: CHANGE TO GET REQUEST
  if (strcmp(url.c_str(), "https://api.bitfinex.com/v1/book/btcusd") == 0) {
    curl_easy_setopt(params.curl, CURLOPT_CUSTOMREQUEST, "GET");
  }
  CURLcode resCurl = curl_easy_perform(params.curl);
  while (resCurl != CURLE_OK) {
    *params.logFile << "Error with cURL: " << curl_easy_strerror(resCurl) << std::endl;
    *params.logFile << "  URL: " << url << std::endl;
    *params.logFile << "  Retry in 2 sec..." << std::endl;
    sleep(2.0);
    readBuffer = "";
    curl_easy_setopt(params.curl, CURLOPT_DNS_CACHE_TIMEOUT, 0);
    resCurl = curl_easy_perform(params.curl);
  }
  json_t* root;
  json_error_t error;
  root = json_loads(readBuffer.c_str(), 0, &error);
  while (!root) {
    *params.logFile << "Error with JSON:\n" << error.text << std::endl;
    *params.logFile << "Buffer:\n" << readBuffer.c_str() << std::endl;
    *params.logFile << "Retrying..." << std::endl;
    sleep(2.0);
    readBuffer = "";
    resCurl = curl_easy_perform(params.curl);
    while (resCurl != CURLE_OK) {
      *params.logFile << "Error with cURL: " << curl_easy_strerror(resCurl) << std::endl;
      *params.logFile << "  URL: " << url << std::endl;
      *params.logFile << "  Retry in 2 sec..." << std::endl;
      sleep(2.0);
      readBuffer = "";
      resCurl = curl_easy_perform(params.curl);
    }
    root = json_loads(readBuffer.c_str(), 0, &error);
  }
  return root;
}

<|endoftext|>


<commit_before>#include <map>

#include <unistd.h>

#include "database.hpp"

#include "cmap.hpp"

#define MAX_COLLECTIONS 100
#define WRITE_INTERVAL  30

using namespace std;

static int backup = -1;

struct Coll {
  pthread_mutex_t mutex;
  string name;
  cmap<int,JSON> documents;
  Coll() : mutex(PTHREAD_MUTEX_INITIALIZER) {}
  void read() { // this function is called only once, by a locked piece of code
    JSON tmp;
    if (!tmp.read_file("database/"+name+".json")) return;
    for (auto& doc : tmp.arr()) documents[doc("_id")] = doc("document");
  }
  void write() {
    JSON tmp(vector<JSON>{});
    pthread_mutex_lock(&mutex);
    for (auto& kv : documents) tmp.emplace_back(move(map<string,JSON>{
      {"_id"      , kv.first},
      {"document" , kv.second}
    }));
    pthread_mutex_unlock(&mutex);
    tmp.write_file("database/"+name+".json");
  }
  int create(JSON&& doc) {
    int id = 1;
    pthread_mutex_lock(&mutex);
    if (documents.size() > 0) id += documents.max_key();
    documents[id] = move(doc);
    pthread_mutex_unlock(&mutex);
    return id;
  }
  bool retrieve(int id, JSON& doc) {
    pthread_mutex_lock(&mutex);
    auto it = documents.find(id);
    if (it == documents.end()) {
      doc.setnull();
      pthread_mutex_unlock(&mutex);
      return false;
    }
    doc = it->second;
    pthread_mutex_unlock(&mutex);
    return true;
  }
  JSON retrieve(const JSON& filter) {
    JSON ans(vector<JSON>{});
    pthread_mutex_lock(&mutex);
    for (auto& kv : documents) if (filter.issubobj(kv.second)) {
      JSON tmp = kv.second;
      tmp["id"] = kv.first;
      ans.push_back(move(tmp));
    }
    pthread_mutex_unlock(&mutex);
    return ans;
  }
  JSON retrieve_page(unsigned p, unsigned ps) {
    JSON ans(vector<JSON>{});
    pthread_mutex_lock(&mutex);
    if (!ps) p = 0, ps = documents.size();
    auto it = documents.at(p*ps);
    for (int i = 0; i < ps && it != documents.end(); i++, it++) {
      JSON tmp = it->second;
      tmp["id"] = it->first;
      ans.push_back(move(tmp));
    }
    pthread_mutex_unlock(&mutex);
    return ans;
  }
  bool update(int id, JSON&& doc) {
    pthread_mutex_lock(&mutex);
    auto it = documents.find(id);
    if (it == documents.end()) {
      pthread_mutex_unlock(&mutex);
      return false;
    }
    it->second = move(doc);
    pthread_mutex_unlock(&mutex);
    return true;
  }
  bool update(const Database::Updater& upd, int id) {
    bool ans = false;
    pthread_mutex_lock(&mutex);
    auto it = documents.find(id);
    if (it != documents.end()) {
      ans = upd(*it);
      pthread_mutex_unlock(&mutex);
      return ans;
    }
    for (auto& kv : documents) ans = ans || upd(kv);
    pthread_mutex_unlock(&mutex);
    return ans;
  }
  bool destroy(int id) {
    pthread_mutex_lock(&mutex);
    auto it = documents.find(id);
    if (it == documents.end()) {
      pthread_mutex_unlock(&mutex);
      return false;
    }
    documents.erase(it);
    pthread_mutex_unlock(&mutex);
    return true;
  }
};
static Coll collection[MAX_COLLECTIONS];
static int ncolls = 0;
static pthread_mutex_t colls_mutex = PTHREAD_MUTEX_INITIALIZER;
static int get(const string& name) {
  static map<string,int> colls;
  int i;
  pthread_mutex_lock(&colls_mutex);
  auto it = colls.find(name);
  if (it != colls.end()) {
    i = it->second;
    pthread_mutex_unlock(&colls_mutex);
    return i;
  }
  i = ncolls++;
  colls[name] = i;
  collection[i].name = name;
  collection[i].read();
  pthread_mutex_unlock(&colls_mutex);
  return i;
}

// thread
static bool quit = false;
static pthread_t db;
static void do_backup() {
  if (backup < 0) return;
  stringstream ss;
  ss << "backup" << backup;
  system(("mkdir -p database/"+ss.str()).c_str());
  system(("cp database/*.json database/"+ss.str()).c_str());
  backup = 1-backup;
}
static void update() {
  int nc;
  pthread_mutex_lock(&colls_mutex);
  nc = ncolls;
  pthread_mutex_unlock(&colls_mutex);
  for (int i = 0; i < nc; i++) collection[i].write();
  do_backup();
  sync();
}
static void* thread(void*) {
  static time_t upd = 0;
  while (!quit) {
    if (upd <= time(nullptr)) {
      update();
      upd = time(nullptr)+WRITE_INTERVAL;
    }
    usleep(100000);
  }
}

namespace Database {

Collection::Collection(const string& name) : collid(get(name)) {

}

int Collection::create(const JSON& document) {
  JSON tmp(document);
  return collection[collid].create(move(tmp));
}

int Collection::create(JSON&& document) {
  return collection[collid].create(move(document));
}

JSON Collection::retrieve(int docid) {
  JSON ans;
  collection[collid].retrieve(docid,ans);
  return ans;
}

bool Collection::retrieve(int docid, JSON& document) {
  return collection[collid].retrieve(docid,document);
}

JSON Collection::retrieve(const JSON& filter) {
  return collection[collid].retrieve(filter);
}

JSON Collection::retrieve_page(unsigned page,unsigned page_size) {
  return collection[collid].retrieve_page(page,page_size);
}

bool Collection::update(int docid, const JSON& document) {
  JSON tmp(document);
  return collection[collid].update(docid,move(tmp));
}

bool Collection::update(int docid, JSON&& document) {
  return collection[collid].update(docid,move(document));
}

bool Collection::update(const Updater& upd, int docid) {
  return collection[collid].update(upd,docid);
}

bool Collection::destroy(int docid) {
  return collection[collid].destroy(docid);
}

void init(bool backup) {
  if (backup) ::backup = 0;
  system("mkdir -p database");
  pthread_create(&db,nullptr,thread,nullptr);
}

void close() {
  quit = true;
  pthread_join(db,nullptr);
  update();
}

} // namespace Database
<commit_msg>Fixing bug<commit_after>#include <map>

#include <unistd.h>

#include "database.hpp"

#include "cmap.hpp"

#define MAX_COLLECTIONS 100
#define WRITE_INTERVAL  30

using namespace std;

static int backup = -1;

struct Coll {
  pthread_mutex_t mutex;
  string name;
  cmap<int,JSON> documents;
  Coll() : mutex(PTHREAD_MUTEX_INITIALIZER) {}
  void read() { // this function is called only once, by a locked piece of code
    JSON tmp;
    if (!tmp.read_file("database/"+name+".json")) return;
    for (auto& doc : tmp.arr()) documents[doc("_id")] = doc("document");
  }
  void write() {
    JSON tmp(vector<JSON>{});
    pthread_mutex_lock(&mutex);
    for (auto& kv : documents) tmp.emplace_back(move(map<string,JSON>{
      {"_id"      , kv.first},
      {"document" , kv.second}
    }));
    pthread_mutex_unlock(&mutex);
    tmp.write_file("database/"+name+".json");
  }
  int create(JSON&& doc) {
    int id = 1;
    pthread_mutex_lock(&mutex);
    if (documents.size() > 0) id += documents.max_key();
    documents[id] = move(doc);
    pthread_mutex_unlock(&mutex);
    return id;
  }
  bool retrieve(int id, JSON& doc) {
    pthread_mutex_lock(&mutex);
    auto it = documents.find(id);
    if (it == documents.end()) {
      doc.setnull();
      pthread_mutex_unlock(&mutex);
      return false;
    }
    doc = it->second;
    pthread_mutex_unlock(&mutex);
    return true;
  }
  JSON retrieve(const JSON& filter) {
    JSON ans(vector<JSON>{});
    pthread_mutex_lock(&mutex);
    for (auto& kv : documents) if (filter.issubobj(kv.second)) {
      JSON tmp = kv.second;
      tmp["id"] = kv.first;
      ans.push_back(move(tmp));
    }
    pthread_mutex_unlock(&mutex);
    return ans;
  }
  JSON retrieve_page(unsigned p, unsigned ps) {
    JSON ans(vector<JSON>{});
    pthread_mutex_lock(&mutex);
    if (!ps) p = 0, ps = documents.size();
    auto it = documents.at(p*ps);
    for (int i = 0; i < ps && it != documents.end(); i++, it++) {
      JSON tmp = it->second;
      tmp["id"] = it->first;
      ans.push_back(move(tmp));
    }
    pthread_mutex_unlock(&mutex);
    return ans;
  }
  bool update(int id, JSON&& doc) {
    pthread_mutex_lock(&mutex);
    auto it = documents.find(id);
    if (it == documents.end()) {
      pthread_mutex_unlock(&mutex);
      return false;
    }
    it->second = move(doc);
    pthread_mutex_unlock(&mutex);
    return true;
  }
  bool update(const Database::Updater& upd, int id) {
    bool ans = false;
    pthread_mutex_lock(&mutex);
    auto it = documents.find(id);
    if (it != documents.end()) {
      ans = upd(*it);
      pthread_mutex_unlock(&mutex);
      return ans;
    }
    for (auto& kv : documents) ans = upd(kv) || ans;
    pthread_mutex_unlock(&mutex);
    return ans;
  }
  bool destroy(int id) {
    pthread_mutex_lock(&mutex);
    auto it = documents.find(id);
    if (it == documents.end()) {
      pthread_mutex_unlock(&mutex);
      return false;
    }
    documents.erase(it);
    pthread_mutex_unlock(&mutex);
    return true;
  }
};
static Coll collection[MAX_COLLECTIONS];
static int ncolls = 0;
static pthread_mutex_t colls_mutex = PTHREAD_MUTEX_INITIALIZER;
static int get(const string& name) {
  static map<string,int> colls;
  int i;
  pthread_mutex_lock(&colls_mutex);
  auto it = colls.find(name);
  if (it != colls.end()) {
    i = it->second;
    pthread_mutex_unlock(&colls_mutex);
    return i;
  }
  i = ncolls++;
  colls[name] = i;
  collection[i].name = name;
  collection[i].read();
  pthread_mutex_unlock(&colls_mutex);
  return i;
}

// thread
static bool quit = false;
static pthread_t db;
static void do_backup() {
  if (backup < 0) return;
  stringstream ss;
  ss << "backup" << backup;
  system(("mkdir -p database/"+ss.str()).c_str());
  system(("cp database/*.json database/"+ss.str()).c_str());
  backup = 1-backup;
}
static void update() {
  int nc;
  pthread_mutex_lock(&colls_mutex);
  nc = ncolls;
  pthread_mutex_unlock(&colls_mutex);
  for (int i = 0; i < nc; i++) collection[i].write();
  do_backup();
  sync();
}
static void* thread(void*) {
  static time_t upd = 0;
  while (!quit) {
    if (upd <= time(nullptr)) {
      update();
      upd = time(nullptr)+WRITE_INTERVAL;
    }
    usleep(100000);
  }
}

namespace Database {

Collection::Collection(const string& name) : collid(get(name)) {

}

int Collection::create(const JSON& document) {
  JSON tmp(document);
  return collection[collid].create(move(tmp));
}

int Collection::create(JSON&& document) {
  return collection[collid].create(move(document));
}

JSON Collection::retrieve(int docid) {
  JSON ans;
  collection[collid].retrieve(docid,ans);
  return ans;
}

bool Collection::retrieve(int docid, JSON& document) {
  return collection[collid].retrieve(docid,document);
}

JSON Collection::retrieve(const JSON& filter) {
  return collection[collid].retrieve(filter);
}

JSON Collection::retrieve_page(unsigned page,unsigned page_size) {
  return collection[collid].retrieve_page(page,page_size);
}

bool Collection::update(int docid, const JSON& document) {
  JSON tmp(document);
  return collection[collid].update(docid,move(tmp));
}

bool Collection::update(int docid, JSON&& document) {
  return collection[collid].update(docid,move(document));
}

bool Collection::update(const Updater& upd, int docid) {
  return collection[collid].update(upd,docid);
}

bool Collection::destroy(int docid) {
  return collection[collid].destroy(docid);
}

void init(bool backup) {
  if (backup) ::backup = 0;
  system("mkdir -p database");
  pthread_create(&db,nullptr,thread,nullptr);
}

void close() {
  quit = true;
  pthread_join(db,nullptr);
  update();
}

} // namespace Database
<|endoftext|>


<commit_before>#include "dbg_path.h"
#include <deque>
#include <algorithm>

namespace vscode
{
	fs::path path_uncomplete(const fs::path& p, const fs::path& base, std::error_code& ec)
	{
		fs::path pp = path_normalize(p);
		if (pp == base)
			return "./";
		fs::path from_path, from_base, output;
		fs::path::iterator path_it = pp.begin(), path_end = pp.end();
		fs::path::iterator base_it = base.begin(), base_end = base.end();

		if ((path_it == path_end) || (base_it == base_end))
		{
			ec = std::make_error_code(std::generic_errno::not_a_directory);
			return p;
		}

#ifdef WIN32
		if (*path_it != *base_it)
			return pp;
		++path_it, ++base_it;
#endif

		const std::string _dot = std::string(1, fs::dot<fs::path>::value);
		const std::string _dots = std::string(2, fs::dot<fs::path>::value);
		const std::string _sep = std::string(1, fs::slash<fs::path>::value);

		while (true) {
			if ((path_it == path_end) || (base_it == base_end) || (*path_it != *base_it)) {
				for (; base_it != base_end; ++base_it) {
					if (*base_it == _dot)
						continue;
					else if (*base_it == _sep)
						continue;

					output /= "../";
				}
				fs::path::iterator path_it_start = path_it;
				for (; path_it != path_end; ++path_it) {

					if (path_it != path_it_start)
						output /= "/";

					if (*path_it == _dot)
						continue;
					if (*path_it == _sep)
						continue;

					output /= *path_it;
				}

				break;
			}
			from_path /= fs::path(*path_it);
			from_base /= fs::path(*base_it);
			++path_it, ++base_it;
		}

		return output;
	}

	fs::path path_normalize(const fs::path& p)
	{
		fs::path result = p.root_path();
		std::deque<std::string> stack;
		for (auto e : p.relative_path()) {
			if (e == ".." && !stack.empty()) {
				stack.pop_back();
			}
			else if (e != ".") {
				stack.push_back(e);
			}
		}
		for (auto e : stack) {
			result /= e;
		}
		std::string ret = result.string();
		std::string tmp;
		tmp.resize(ret.size());
		std::transform(ret.begin(), ret.end(), tmp.begin(), tolower);
		return tmp;
	}

	bool path_is_subpath(const fs::path& path, const fs::path& base)
	{
		fs::path npath = path_normalize(path);
		fs::path::iterator path_it = npath.begin(), path_end = npath.end();
		fs::path::iterator base_it = base.begin(), base_end = base.end();

		for (;;)
		{
			if (base_it == base_end) {
				return true;
			}
			if (path_it == path_end) {
				return false;
			}
			if (*path_it != *base_it) {
				return false;
			}
			++path_it;
			++base_it;
		}
		return false;
	}
}
<commit_msg>修正path_normalize的一个错误<commit_after>#include "dbg_path.h"
#include <deque>
#include <algorithm>

namespace vscode
{
	fs::path path_uncomplete(const fs::path& p, const fs::path& base, std::error_code& ec)
	{
		fs::path pp = path_normalize(p);
		if (pp == base)
			return "./";
		fs::path from_path, from_base, output;
		fs::path::iterator path_it = pp.begin(), path_end = pp.end();
		fs::path::iterator base_it = base.begin(), base_end = base.end();

		if ((path_it == path_end) || (base_it == base_end))
		{
			ec = std::make_error_code(std::generic_errno::not_a_directory);
			return p;
		}

#ifdef WIN32
		if (*path_it != *base_it)
			return pp;
		++path_it, ++base_it;
#endif

		const std::string _dot = std::string(1, fs::dot<fs::path>::value);
		const std::string _dots = std::string(2, fs::dot<fs::path>::value);
		const std::string _sep = std::string(1, fs::slash<fs::path>::value);

		while (true) {
			if ((path_it == path_end) || (base_it == base_end) || (*path_it != *base_it)) {
				for (; base_it != base_end; ++base_it) {
					if (*base_it == _dot)
						continue;
					else if (*base_it == _sep)
						continue;

					output /= "../";
				}
				fs::path::iterator path_it_start = path_it;
				for (; path_it != path_end; ++path_it) {

					if (path_it != path_it_start)
						output /= "/";

					if (*path_it == _dot)
						continue;
					if (*path_it == _sep)
						continue;

					output /= *path_it;
				}

				break;
			}
			from_path /= fs::path(*path_it);
			from_base /= fs::path(*base_it);
			++path_it, ++base_it;
		}

		return output;
	}

	fs::path path_normalize(const fs::path& p)
	{
		fs::path result = p.root_path();
		std::deque<std::string> stack;
		for (auto e : p.relative_path()) {
			if (e == ".." && !stack.empty() && stack.back() != "..") {
				stack.pop_back();
			}
			else if (e != ".") {
				stack.push_back(e);
			}
		}
		for (auto e : stack) {
			result /= e;
		}
		std::string ret = result.string();
		std::string tmp;
		tmp.resize(ret.size());
		std::transform(ret.begin(), ret.end(), tmp.begin(), tolower);
		return tmp;
	}

	bool path_is_subpath(const fs::path& path, const fs::path& base)
	{
		fs::path npath = path_normalize(path);
		fs::path::iterator path_it = npath.begin(), path_end = npath.end();
		fs::path::iterator base_it = base.begin(), base_end = base.end();

		for (;;)
		{
			if (base_it == base_end) {
				return true;
			}
			if (path_it == path_end) {
				return false;
			}
			if (*path_it != *base_it) {
				return false;
			}
			++path_it;
			++base_it;
		}
		return false;
	}
}
<|endoftext|>