<commit_before>#include <netdb.h>
#include <unistd.h>
#include <sys/fcntl.h>

#include "client.h"
#include "duckchat.h"

// Variables
struct sockaddr_in client_addr;
struct sockaddr_in server_addr;
int client_socket;
struct addrinfo *server_info;
char *channel;

// Prints an error message and exits the program.
void Error(const char *msg) {
  std::cerr << msg << std::endl;
  exit(1);
}

// Connects to the server at a the given port.
void Connect(char *domain, const char *port) {
  std::cout << "Connecting to " << domain << std::endl;

  struct addrinfo hints;
  struct addrinfo *server_info_tmp;
  int status;

  memset(&hints, 0, sizeof(hints));
  hints.ai_family = AF_UNSPEC;
  hints.ai_socktype = SOCK_DGRAM;
  hints.ai_protocol = 0;

  if ((status = getaddrinfo(domain, port, &hints, &server_info_tmp)) != 0) {
    std::cerr << "client: unable to resolve address: " << gai_strerror(status) << std::endl;
    exit(1);
  }

  // getaddrinfo() returns a list of address structures into server_info_tmp.
  // Try each address until we successfully connect().
  // If socket() (or connect()) fails, close the socket and try the next address.

  for (server_info = server_info_tmp; server_info != NULL; server_info = server_info->ai_next) {
    if ((client_socket = socket(server_info->ai_family, server_info->ai_socktype, server_info->ai_protocol)) < 0) {
      continue;
    }
    if (connect(client_socket, server_info->ai_addr, server_info->ai_addrlen) != -1) {
      fcntl(client_socket, F_SETFL, O_NONBLOCK);
      break; // Success
    }
    close(client_socket);
  }

  if (server_info == NULL) {
    Error("client: all sockets failed to connect");
  }
}

// Sends a message to all users in on the active channel.
int RequestSay(const char *message) {
  struct request_say say;
  memset(&say, 0, sizeof(say));
  say.req_type = REQ_SAY;
  strncpy(say.req_text, message, SAY_MAX);
  strncpy(say.req_channel, channel, CHANNEL_MAX);

  if (sendto(client_socket, &say, sizeof(say), 0, server_info->ai_addr, server_info->ai_addrlen) < 0) {
    Error("client: failed to send message\n");
  }

  return 0;
}

// Sends login requests to the server.
int RequestLogin(char *username) {
  struct request_login login;
  memset(&login, 0, sizeof(login));
  login.req_type = REQ_LOGIN;
  strncpy(login.req_username, username, USERNAME_MAX);

  size_t message_size = sizeof(struct request_login);

  if (sendto(client_socket, &login, message_size, 0, server_info->ai_addr, server_info->ai_addrlen) < 0) {
    Error("client: failed to request login\n");
  }

  return 0;
}

// Sends logout requests to the server.
int RequestLogout() {
  struct request_logout logout;
  memset((char *) &logout, 0, sizeof(logout));
  logout.req_type = REQ_LOGOUT;

  size_t message_size = sizeof(struct request_logout);

  if (sendto(client_socket, &logout, message_size, 0, server_info->ai_addr, server_info->ai_addrlen) < 0) {
    Error("client: failed to request logout\n");
  }

  return 0;
}

// Sends join requests to the server.
int RequestJoin(char *channel) {
  struct request_join join;
  memset((char *) &join, 0, sizeof(join));
  join.req_type = REQ_JOIN;
  strncpy(join.req_channel, channel, CHANNEL_MAX);

  size_t message_size = sizeof(struct request_join);

  if (sendto(client_socket, &join, message_size, 0, server_info->ai_addr, server_info->ai_addrlen) < 0) {
    Error("client: failed to request join\n");
  }

  return 0;
}

// Splits strings around spaces.
std::vector<std::string> StringSplit(std::string input) {
  std::istringstream iss(input);
  std::vector<std::string> result{std::istream_iterator<std::string>{iss}, std::istream_iterator<std::string>{}};

  return result;
}

// Splits strings around spaces.
std::vector<std::string> SplitString(char *input, char delimiter) {
  std::vector<std::string> result;
  std::string word = "";

  size_t input_size = strlen(input);
  for (size_t i = 0; i < input_size; i++) {
    if (input[i] != delimiter) {
      word += input[i];
    } else {
      result.push_back(word);
      word = "";
    }
  }
  result.push_back(word);

  return result;
}

void StripChar(char *input, char c) {
  size_t size = strlen(input);
  for (size_t i = 0; i < size; i++) {
    if (input[i] == c) {
      input[i] = '\0';
    }
  }
}

// Processes the input string to decide what type of command it is.
bool ProcessInput(std::string input) {
  std::vector<std::string> inputs = StringSplit(input);
  bool result = true;

  if (inputs[0] == "/exit") {
    RequestLogout();
    result = false;
  } else if (inputs[0] == "/list") {

  } else if (inputs[0] == "/join") {

  } else if (inputs[0] == "/leave") {

  } else if (inputs[0] == "/who") {

  } else if (inputs[0] == "/switch") {

  } else {
    std::cout << "\n*Unknown command" << std::endl;
  }

  return result;
}

int main(int argc, char *argv[]) {
  char *domain;
  char *port_str;
  int port_num;
  char *username;
//  std::string input;
  char tmp_buffer[SAY_MAX];
  memset(&tmp_buffer, 0, SAY_MAX);

//  struct timeval timeout;
  fd_set read_set;
//  int file_desc = 0;
  int result;
  char receive_buffer[kBufferSize];
  memset(&receive_buffer, 0, kBufferSize);

  char stdin_buffer[kBufferSize];
  memset(&stdin_buffer, 0, kBufferSize);

  if (argc < 4) {
    Error("usage: client [server name] [port] [username]");
  }

  domain = argv[1];
  port_str = argv[2];
  port_num = atoi(argv[2]);
  username = argv[3];

  if (strlen(domain) > UNIX_PATH_MAX) {
    Error("client: server name must be less than 108 characters");
  }

  if (port_num < 0 || port_num > 65535) {
    Error("client: port number must be between 0 and 65535");
  }

  if (strlen(username) > USERNAME_MAX) {
    Error("client: username must be less than 32 characters");
  }

  Connect(domain, port_str);

  RequestLogin(username);

  channel = (char *) "Common";
  RequestJoin(channel);

  // TODO handle response from send

  std::cout << ">" << std::flush;

  while (1) {
    FD_ZERO(&read_set);
    FD_SET(client_socket, &read_set);
    FD_SET(STDIN_FILENO, &read_set);

    if ((result = select(client_socket + 1, &read_set, NULL, NULL, NULL)) < 0) {
      Error("client: problem using select");
    }

    if (result > 0) {
      if (FD_ISSET(STDIN_FILENO, &read_set)) {
        int read_stdin_size = read(STDIN_FILENO, stdin_buffer, kBufferSize);

        if (read_stdin_size != 0) {
          if (stdin_buffer[0] == '/') {
            ProcessInput(stdin_buffer);
          } else {
            // Send chat messages
            StripChar(stdin_buffer, '\n');
            RequestSay(stdin_buffer);
          }
        }

        memset(&stdin_buffer, 0, kBufferSize);
      } // end of if STDIN_FILENO

      if (FD_ISSET(client_socket, &read_set)) {
        // Socket has data
        int read_size = read(client_socket, receive_buffer, kBufferSize);

        if (read_size != 0) {
          // TODO capture user input, store, clean input, then print buffer, afterward replace input
          struct text message;
          memcpy(&message, receive_buffer, sizeof(struct text));
          text_t text_type = message.txt_type;

          switch (text_type) {
            case TXT_SAY:
              struct text_say say;
              memcpy(&say, receive_buffer, sizeof(struct text_say));
//              read(STDIN_FILENO, tmp_buffer, sizeof(tmp_buffer));
              std::cout << "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b";
              std::cout << "[" << say.txt_channel << "]" << "[" << say.txt_username << "]: " << say.txt_text << std::endl;
              std::cout << ">" << tmp_buffer << std::flush;
              memset(&tmp_buffer, 0, SAY_MAX);
              break;
            default:
              break;
          }
        }

        memset(&receive_buffer, 0, SAY_MAX);
      } // end of if client_socket

    } // end of if result

  } // end of while

  return 0;
}<commit_msg>readsome again<commit_after>#include <netdb.h>
#include <unistd.h>
#include <sys/fcntl.h>

#include "client.h"
#include "duckchat.h"

// Variables
struct sockaddr_in client_addr;
struct sockaddr_in server_addr;
int client_socket;
struct addrinfo *server_info;
char *channel;

// Prints an error message and exits the program.
void Error(const char *msg) {
  std::cerr << msg << std::endl;
  exit(1);
}

// Connects to the server at a the given port.
void Connect(char *domain, const char *port) {
  std::cout << "Connecting to " << domain << std::endl;

  struct addrinfo hints;
  struct addrinfo *server_info_tmp;
  int status;

  memset(&hints, 0, sizeof(hints));
  hints.ai_family = AF_UNSPEC;
  hints.ai_socktype = SOCK_DGRAM;
  hints.ai_protocol = 0;

  if ((status = getaddrinfo(domain, port, &hints, &server_info_tmp)) != 0) {
    std::cerr << "client: unable to resolve address: " << gai_strerror(status) << std::endl;
    exit(1);
  }

  // getaddrinfo() returns a list of address structures into server_info_tmp.
  // Try each address until we successfully connect().
  // If socket() (or connect()) fails, close the socket and try the next address.

  for (server_info = server_info_tmp; server_info != NULL; server_info = server_info->ai_next) {
    if ((client_socket = socket(server_info->ai_family, server_info->ai_socktype, server_info->ai_protocol)) < 0) {
      continue;
    }
    if (connect(client_socket, server_info->ai_addr, server_info->ai_addrlen) != -1) {
      fcntl(client_socket, F_SETFL, O_NONBLOCK);
      break; // Success
    }
    close(client_socket);
  }

  if (server_info == NULL) {
    Error("client: all sockets failed to connect");
  }
}

// Sends a message to all users in on the active channel.
int RequestSay(const char *message) {
  struct request_say say;
  memset(&say, 0, sizeof(say));
  say.req_type = REQ_SAY;
  strncpy(say.req_text, message, SAY_MAX);
  strncpy(say.req_channel, channel, CHANNEL_MAX);

  if (sendto(client_socket, &say, sizeof(say), 0, server_info->ai_addr, server_info->ai_addrlen) < 0) {
    Error("client: failed to send message\n");
  }

  return 0;
}

// Sends login requests to the server.
int RequestLogin(char *username) {
  struct request_login login;
  memset(&login, 0, sizeof(login));
  login.req_type = REQ_LOGIN;
  strncpy(login.req_username, username, USERNAME_MAX);

  size_t message_size = sizeof(struct request_login);

  if (sendto(client_socket, &login, message_size, 0, server_info->ai_addr, server_info->ai_addrlen) < 0) {
    Error("client: failed to request login\n");
  }

  return 0;
}

// Sends logout requests to the server.
int RequestLogout() {
  struct request_logout logout;
  memset((char *) &logout, 0, sizeof(logout));
  logout.req_type = REQ_LOGOUT;

  size_t message_size = sizeof(struct request_logout);

  if (sendto(client_socket, &logout, message_size, 0, server_info->ai_addr, server_info->ai_addrlen) < 0) {
    Error("client: failed to request logout\n");
  }

  return 0;
}

// Sends join requests to the server.
int RequestJoin(char *channel) {
  struct request_join join;
  memset((char *) &join, 0, sizeof(join));
  join.req_type = REQ_JOIN;
  strncpy(join.req_channel, channel, CHANNEL_MAX);

  size_t message_size = sizeof(struct request_join);

  if (sendto(client_socket, &join, message_size, 0, server_info->ai_addr, server_info->ai_addrlen) < 0) {
    Error("client: failed to request join\n");
  }

  return 0;
}

// Splits strings around spaces.
std::vector<std::string> StringSplit(std::string input) {
  std::istringstream iss(input);
  std::vector<std::string> result{std::istream_iterator<std::string>{iss}, std::istream_iterator<std::string>{}};

  return result;
}

// Splits strings around spaces.
std::vector<std::string> SplitString(char *input, char delimiter) {
  std::vector<std::string> result;
  std::string word = "";

  size_t input_size = strlen(input);
  for (size_t i = 0; i < input_size; i++) {
    if (input[i] != delimiter) {
      word += input[i];
    } else {
      result.push_back(word);
      word = "";
    }
  }
  result.push_back(word);

  return result;
}

void StripChar(char *input, char c) {
  size_t size = strlen(input);
  for (size_t i = 0; i < size; i++) {
    if (input[i] == c) {
      input[i] = '\0';
    }
  }
}

// Processes the input string to decide what type of command it is.
bool ProcessInput(std::string input) {
  std::vector<std::string> inputs = StringSplit(input);
  bool result = true;

  if (inputs[0] == "/exit") {
    RequestLogout();
    result = false;
  } else if (inputs[0] == "/list") {

  } else if (inputs[0] == "/join") {

  } else if (inputs[0] == "/leave") {

  } else if (inputs[0] == "/who") {

  } else if (inputs[0] == "/switch") {

  } else {
    std::cout << "\n*Unknown command" << std::endl;
  }

  return result;
}

int main(int argc, char *argv[]) {
  char *domain;
  char *port_str;
  int port_num;
  char *username;
//  std::string input;
  char tmp_buffer[SAY_MAX];
  memset(&tmp_buffer, 0, SAY_MAX);

//  struct timeval timeout;
  fd_set read_set;
//  int file_desc = 0;
  int result;
  char receive_buffer[kBufferSize];
  memset(&receive_buffer, 0, kBufferSize);

  char stdin_buffer[kBufferSize];
  memset(&stdin_buffer, 0, kBufferSize);

  if (argc < 4) {
    Error("usage: client [server name] [port] [username]");
  }

  domain = argv[1];
  port_str = argv[2];
  port_num = atoi(argv[2]);
  username = argv[3];

  if (strlen(domain) > UNIX_PATH_MAX) {
    Error("client: server name must be less than 108 characters");
  }

  if (port_num < 0 || port_num > 65535) {
    Error("client: port number must be between 0 and 65535");
  }

  if (strlen(username) > USERNAME_MAX) {
    Error("client: username must be less than 32 characters");
  }

  Connect(domain, port_str);

  RequestLogin(username);

  channel = (char *) "Common";
  RequestJoin(channel);

  // TODO handle response from send

  std::cout << ">" << std::flush;

  while (1) {
    FD_ZERO(&read_set);
    FD_SET(client_socket, &read_set);
    FD_SET(STDIN_FILENO, &read_set);

    if ((result = select(client_socket + 1, &read_set, NULL, NULL, NULL)) < 0) {
      Error("client: problem using select");
    }

    if (result > 0) {
      if (FD_ISSET(STDIN_FILENO, &read_set)) {
        int read_stdin_size = read(STDIN_FILENO, stdin_buffer, kBufferSize);

        if (read_stdin_size != 0) {
          if (stdin_buffer[0] == '/') {
            ProcessInput(stdin_buffer);
          } else {
            // Send chat messages
            StripChar(stdin_buffer, '\n');
            RequestSay(stdin_buffer);
          }
        }

        memset(&stdin_buffer, 0, kBufferSize);
      } // end of if STDIN_FILENO

      if (FD_ISSET(client_socket, &read_set)) {
        // Socket has data
        int read_size = read(client_socket, receive_buffer, kBufferSize);

        if (read_size != 0) {
          // TODO capture user input, store, clean input, then print buffer, afterward replace input
          struct text message;
          memcpy(&message, receive_buffer, sizeof(struct text));
          text_t text_type = message.txt_type;

          switch (text_type) {
            case TXT_SAY:
              struct text_say say;
              memcpy(&say, receive_buffer, sizeof(struct text_say));
              std::cin.readsome(tmp_buffer, sizeof(tmp_buffer));
              std::cout << "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b";
              std::cout << "[" << say.txt_channel << "]" << "[" << say.txt_username << "]: " << say.txt_text << std::endl;
              std::cout << ">" << tmp_buffer << std::flush;
              memset(&tmp_buffer, 0, SAY_MAX);
              break;
            default:
              break;
          }
        }

        memset(&receive_buffer, 0, SAY_MAX);
      } // end of if client_socket

    } // end of if result

  } // end of while

  return 0;
}<|endoftext|>


<commit_before>////////////////////////////////////////////////////////////////////////////////
/// DISCLAIMER
///
/// Copyright 2014-2016 ArangoDB GmbH, Cologne, Germany
/// Copyright 2004-2014 triAGENS GmbH, Cologne, Germany
///
/// 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.
///
/// Copyright holder is ArangoDB GmbH, Cologne, Germany
///
/// @author Kaveh Vahedipour
////////////////////////////////////////////////////////////////////////////////

#include "Supervision.h"

#include "Agent.h"
#include "CleanOutServer.h"
#include "FailedLeader.h"
#include "FailedServer.h"
#include "Job.h"
#include "Store.h"

#include "Basics/ConditionLocker.h"
#include "VocBase/server.h"

#include <thread>
using namespace arangodb;

using namespace arangodb::consensus;

std::string Supervision::_agencyPrefix = "/arango";

Supervision::Supervision()
    : arangodb::Thread("Supervision"),
      _agent(nullptr),
      _snapshot("Supervision"),
      _frequency(5),
      _gracePeriod(15),
      _jobId(0),
      _jobIdMax(0) {}

Supervision::~Supervision() { shutdown(); };

void Supervision::wakeUp() {
  TRI_ASSERT(_agent != nullptr);
  if (!this->isStopping()) {
    _snapshot = _agent->readDB().get(_agencyPrefix);
  }
  _cv.signal();
}

static std::string const syncPrefix = "/Sync/ServerStates/";
static std::string const healthPrefix = "/Supervision/Health/";
static std::string const planDBServersPrefix = "/Plan/DBServers";
static std::string const planCoordinatorsPrefix = "/Plan/Coordinators";

std::vector<check_t> Supervision::checkDBServers() {
  std::vector<check_t> ret;
  Node::Children const& machinesPlanned =
      _snapshot(planDBServersPrefix).children();

  for (auto const& machine : machinesPlanned) {

    bool good = false;
    std::string lastHeartbeatTime, lastHeartbeatStatus, lastHeartbeatAcked,
      lastStatus, heartbeatTime, heartbeatStatus, serverID;

    serverID        = machine.first;
    heartbeatTime   = _snapshot(syncPrefix + serverID + "/time").toJson();
    heartbeatStatus = _snapshot(syncPrefix + serverID + "/status").toJson();

    try {           // Existing
      lastHeartbeatTime =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatSent").toJson();
      lastHeartbeatStatus =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatStatus").toJson();
      lastHeartbeatAcked =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatAcked").toJson();
      lastStatus = _snapshot(healthPrefix + serverID + "/Status").toJson();
      if (lastHeartbeatTime != heartbeatTime) { // Update
        good = true;
      }
    } catch (...) { // New server
      good = true;
    }

    query_t report = std::make_shared<Builder>();
    report->openArray();
    report->openArray();
    report->openObject();
    report->add(_agencyPrefix + healthPrefix + serverID,
                VPackValue(VPackValueType::Object));
    report->add("LastHeartbeatSent", VPackValue(heartbeatTime));
    report->add("LastHeartbeatStatus", VPackValue(heartbeatStatus));

    if (good) {
      report->add("LastHeartbeatAcked",
                  VPackValue(
                    timepointToString(std::chrono::system_clock::now())));
      report->add("Status", VPackValue("GOOD"));
    } else {
      std::chrono::seconds t{0};
      t = std::chrono::duration_cast<std::chrono::seconds>(
        std::chrono::system_clock::now()-stringToTimepoint(lastHeartbeatAcked));
      if (t.count() > _gracePeriod) {  // Failure
        if (lastStatus == "BAD") {
          report->add("Status", VPackValue("FAILED"));
          FailedServer fsj(_snapshot, _agent, std::to_string(_jobId++),
                           "supervision", _agencyPrefix, serverID);
        }
      } else {
        report->add("Status", VPackValue("BAD"));
      }
    }

    report->close();
    report->close();
    report->close();
    report->close();
    if (!this->isStopping()) {
      _agent->write(report);
    }

  }

  return ret;
}

std::vector<check_t> Supervision::checkCoordinators() {
  std::vector<check_t> ret;
  Node::Children const& machinesPlanned =
      _snapshot(planCoordinatorsPrefix).children();

  for (auto const& machine : machinesPlanned) {

    bool good = false;
    std::string lastHeartbeatTime, lastHeartbeatStatus, lastHeartbeatAcked,
      lastStatus, heartbeatTime, heartbeatStatus, serverID;

    serverID        = machine.first;
    heartbeatTime   = _snapshot(syncPrefix + serverID + "/time").toJson();
    heartbeatStatus = _snapshot(syncPrefix + serverID + "/status").toJson();

    try {           // Existing
      lastHeartbeatTime =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatSent").toJson();
      lastHeartbeatStatus =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatStatus").toJson();
      lastHeartbeatAcked =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatAcked").toJson();
      lastStatus = _snapshot(healthPrefix + serverID + "/Status").toJson();
      if (lastHeartbeatTime != heartbeatTime) { // Update
        good = true;
      }
    } catch (...) { // New server
      good = true;
    }

    query_t report = std::make_shared<Builder>();
    report->openArray();
    report->openArray();
    report->openObject();
    report->add(_agencyPrefix + healthPrefix + serverID,
                VPackValue(VPackValueType::Object));
    report->add("LastHeartbeatSent", VPackValue(heartbeatTime));
    report->add("LastHeartbeatStatus", VPackValue(heartbeatStatus));

    if (good) {
      report->add("LastHeartbeatAcked",
                  VPackValue(
                    timepointToString(std::chrono::system_clock::now())));
      report->add("Status", VPackValue("GOOD"));
    } else {
      std::chrono::seconds t{0};
      t = std::chrono::duration_cast<std::chrono::seconds>(
        std::chrono::system_clock::now()-stringToTimepoint(lastHeartbeatAcked));
      if (t.count() > _gracePeriod) {  // Failure
        if (lastStatus == "BAD") {
          report->add("Status", VPackValue("FAILED"));
        }
      } else {
        report->add("Status", VPackValue("BAD"));
      }
    }

    report->close();
    report->close();
    report->close();
    report->close();
    if (!this->isStopping()) {
      _agent->write(report);
    }

  }

  return ret;
}

bool Supervision::updateSnapshot() {

  if (_agent == nullptr || this->isStopping()) {
    return false;
  }
  _snapshot = _agent->readDB().get(_agencyPrefix);
  return true;
}

bool Supervision::doChecks(bool timedout) {

  checkDBServers();
  checkCoordinators();
  return true;

}

void Supervision::run() {

  CONDITION_LOCKER(guard, _cv);
  TRI_ASSERT(_agent != nullptr);
  bool timedout = false;

  while (!this->isStopping()) {

    // Get agency prefix after cluster init
    if (_jobId == 0) {
      // We need the agency prefix to work, but it is only initialized by
      // some other server in the cluster. Since the supervision does not
      // make sense at all without other ArangoDB servers, we wait pretty
      // long here before giving up:
      if (!updateAgencyPrefix(1000, 1)) {
        LOG_TOPIC(ERR, Logger::AGENCY)
          << "Cannot get prefix from Agency. Stopping supervision for good.";
        break;
      }
    }

    // Get bunch of job IDs from agency for future jobs
    if (_jobId == 0 || _jobId == _jobIdMax) {
      getUniqueIds();  // cannot fail but only hang
    }

    // Do nothing unless leader
    if (_agent->leading()) {
      timedout = _cv.wait(_frequency * 1000000);  // quarter second
    } else {
      _cv.wait();
    }

    // Do supervision
    updateSnapshot();
    doChecks(timedout);
    workJobs();

  }

}

void Supervision::workJobs() {

  Node::Children const& todos = _snapshot(toDoPrefix).children();
  Node::Children const& pends = _snapshot(pendingPrefix).children();

  if (!todos.empty()) {
    for (auto const& todoEnt : todos) {
      Node const& job = *todoEnt.second;

      try {
        std::string jobType = job("type").getString(),
          jobId = job("jobId").getString(),
          creator = job("creator").getString();
        if (jobType == "failedServer") {
          FailedServer fs(_snapshot, _agent, jobId, creator, _agencyPrefix);
        } else if (jobType == "cleanOutServer") {
          CleanOutServer cos(_snapshot, _agent, jobId, creator, _agencyPrefix);
        }
      } catch (std::exception const& e) {}
    }
  }

  if (!pends.empty()) {
    for (auto const& pendEnt : pends) {
      Node const& job = *pendEnt.second;

      try {
        std::string jobType = job("type").getString(),
          jobId = job("jobId").getString(),
          creator = job("creator").getString();
        if (jobType == "failedServer") {
          FailedServer fs(_snapshot, _agent, jobId, creator, _agencyPrefix);
        } else if (jobType == "cleanOutServer") {
          CleanOutServer cos(_snapshot, _agent, jobId, creator, _agencyPrefix);
        }
      } catch (std::exception const& e) {}
    }
  }

}

// Start thread
bool Supervision::start() {
  Thread::start();
  return true;
}

// Start thread with agent
bool Supervision::start(Agent* agent) {
  _agent = agent;
  _frequency = static_cast<long>(_agent->config().supervisionFrequency);

  return start();
}

// Get agency prefix fron agency
bool Supervision::updateAgencyPrefix (size_t nTries, int intervalSec) {

  // Try nTries to get agency's prefix in intervals
  while (!this->isStopping()) {
    _snapshot = _agent->readDB().get("/");
    if (_snapshot.children().size() > 0) {
      _agencyPrefix = std::string("/") + _snapshot.children().begin()->first;
      LOG_TOPIC(DEBUG, Logger::AGENCY) << "Agency prefix is " << _agencyPrefix;
      return true;
    }
    std::this_thread::sleep_for (std::chrono::seconds(intervalSec));
  }

  // Stand-alone agency
  return false;

}

static std::string const syncLatest = "/Sync/LatestID";
// Get bunch of cluster's unique ids from agency
void Supervision::getUniqueIds() {
  uint64_t latestId;
  // Run forever, supervision does not make sense before the agency data
  // is initialized by some other server...
  while (!this->isStopping()) {
    try {
      latestId = std::stoul(
          _agent->readDB().get(_agencyPrefix + "/Sync/LatestID").slice().toJson());
    } catch (std::exception const&) {
      std::this_thread::sleep_for (std::chrono::seconds(1));
      continue;
    }

    Builder uniq;
    uniq.openArray();
    uniq.openObject();
    uniq.add(_agencyPrefix + syncLatest, VPackValue(latestId + 100000)); // new
    uniq.close();
    uniq.openObject();
    uniq.add(_agencyPrefix + syncLatest, VPackValue(latestId));      // precond
    uniq.close();
    uniq.close();

    auto result = transact(_agent, uniq);
    if (result.indices[0]) {
      _agent->waitFor(result.indices[0]);
      _jobId = latestId;
      _jobIdMax = latestId + 100000;
      return;
    }
  }
}

void Supervision::updateFromAgency() {
  auto const& jobsPending =
      _snapshot("/Supervision/Jobs/Pending").children();

  for (auto const& jobent : jobsPending) {
    auto const& job = *(jobent.second);

    LOG_TOPIC(WARN, Logger::AGENCY)
      << job.name() << " " << job("failed").toJson() << job("");
  }
}

void Supervision::beginShutdown() {
  // Personal hygiene
  Thread::beginShutdown();
}

Store const& Supervision::store() const {
  return _agent->readDB();
}
<commit_msg>fixed Visual Studio warnings<commit_after>////////////////////////////////////////////////////////////////////////////////
/// DISCLAIMER
///
/// Copyright 2014-2016 ArangoDB GmbH, Cologne, Germany
/// Copyright 2004-2014 triAGENS GmbH, Cologne, Germany
///
/// 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.
///
/// Copyright holder is ArangoDB GmbH, Cologne, Germany
///
/// @author Kaveh Vahedipour
////////////////////////////////////////////////////////////////////////////////

#include "Supervision.h"

#include "Agent.h"
#include "CleanOutServer.h"
#include "FailedLeader.h"
#include "FailedServer.h"
#include "Job.h"
#include "Store.h"

#include "Basics/ConditionLocker.h"
#include "VocBase/server.h"

#include <thread>
using namespace arangodb;

using namespace arangodb::consensus;

std::string Supervision::_agencyPrefix = "/arango";

Supervision::Supervision()
    : arangodb::Thread("Supervision"),
      _agent(nullptr),
      _snapshot("Supervision"),
      _frequency(5),
      _gracePeriod(15),
      _jobId(0),
      _jobIdMax(0) {}

Supervision::~Supervision() { shutdown(); };

void Supervision::wakeUp() {
  TRI_ASSERT(_agent != nullptr);
  if (!this->isStopping()) {
    _snapshot = _agent->readDB().get(_agencyPrefix);
  }
  _cv.signal();
}

static std::string const syncPrefix = "/Sync/ServerStates/";
static std::string const healthPrefix = "/Supervision/Health/";
static std::string const planDBServersPrefix = "/Plan/DBServers";
static std::string const planCoordinatorsPrefix = "/Plan/Coordinators";

std::vector<check_t> Supervision::checkDBServers() {
  std::vector<check_t> ret;
  Node::Children const& machinesPlanned =
      _snapshot(planDBServersPrefix).children();

  for (auto const& machine : machinesPlanned) {

    bool good = false;
    std::string lastHeartbeatTime, lastHeartbeatStatus, lastHeartbeatAcked,
      lastStatus, heartbeatTime, heartbeatStatus, serverID;

    serverID        = machine.first;
    heartbeatTime   = _snapshot(syncPrefix + serverID + "/time").toJson();
    heartbeatStatus = _snapshot(syncPrefix + serverID + "/status").toJson();

    try {           // Existing
      lastHeartbeatTime =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatSent").toJson();
      lastHeartbeatStatus =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatStatus").toJson();
      lastHeartbeatAcked =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatAcked").toJson();
      lastStatus = _snapshot(healthPrefix + serverID + "/Status").toJson();
      if (lastHeartbeatTime != heartbeatTime) { // Update
        good = true;
      }
    } catch (...) { // New server
      good = true;
    }

    query_t report = std::make_shared<Builder>();
    report->openArray();
    report->openArray();
    report->openObject();
    report->add(_agencyPrefix + healthPrefix + serverID,
                VPackValue(VPackValueType::Object));
    report->add("LastHeartbeatSent", VPackValue(heartbeatTime));
    report->add("LastHeartbeatStatus", VPackValue(heartbeatStatus));

    if (good) {
      report->add("LastHeartbeatAcked",
                  VPackValue(
                    timepointToString(std::chrono::system_clock::now())));
      report->add("Status", VPackValue("GOOD"));
    } else {
      std::chrono::seconds t{0};
      t = std::chrono::duration_cast<std::chrono::seconds>(
        std::chrono::system_clock::now()-stringToTimepoint(lastHeartbeatAcked));
      if (t.count() > _gracePeriod) {  // Failure
        if (lastStatus == "BAD") {
          report->add("Status", VPackValue("FAILED"));
          FailedServer fsj(_snapshot, _agent, std::to_string(_jobId++),
                           "supervision", _agencyPrefix, serverID);
        }
      } else {
        report->add("Status", VPackValue("BAD"));
      }
    }

    report->close();
    report->close();
    report->close();
    report->close();
    if (!this->isStopping()) {
      _agent->write(report);
    }

  }

  return ret;
}

std::vector<check_t> Supervision::checkCoordinators() {
  std::vector<check_t> ret;
  Node::Children const& machinesPlanned =
      _snapshot(planCoordinatorsPrefix).children();

  for (auto const& machine : machinesPlanned) {

    bool good = false;
    std::string lastHeartbeatTime, lastHeartbeatStatus, lastHeartbeatAcked,
      lastStatus, heartbeatTime, heartbeatStatus, serverID;

    serverID        = machine.first;
    heartbeatTime   = _snapshot(syncPrefix + serverID + "/time").toJson();
    heartbeatStatus = _snapshot(syncPrefix + serverID + "/status").toJson();

    try {           // Existing
      lastHeartbeatTime =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatSent").toJson();
      lastHeartbeatStatus =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatStatus").toJson();
      lastHeartbeatAcked =
        _snapshot(healthPrefix + serverID + "/LastHeartbeatAcked").toJson();
      lastStatus = _snapshot(healthPrefix + serverID + "/Status").toJson();
      if (lastHeartbeatTime != heartbeatTime) { // Update
        good = true;
      }
    } catch (...) { // New server
      good = true;
    }

    query_t report = std::make_shared<Builder>();
    report->openArray();
    report->openArray();
    report->openObject();
    report->add(_agencyPrefix + healthPrefix + serverID,
                VPackValue(VPackValueType::Object));
    report->add("LastHeartbeatSent", VPackValue(heartbeatTime));
    report->add("LastHeartbeatStatus", VPackValue(heartbeatStatus));

    if (good) {
      report->add("LastHeartbeatAcked",
                  VPackValue(
                    timepointToString(std::chrono::system_clock::now())));
      report->add("Status", VPackValue("GOOD"));
    } else {
      std::chrono::seconds t{0};
      t = std::chrono::duration_cast<std::chrono::seconds>(
        std::chrono::system_clock::now()-stringToTimepoint(lastHeartbeatAcked));
      if (t.count() > _gracePeriod) {  // Failure
        if (lastStatus == "BAD") {
          report->add("Status", VPackValue("FAILED"));
        }
      } else {
        report->add("Status", VPackValue("BAD"));
      }
    }

    report->close();
    report->close();
    report->close();
    report->close();
    if (!this->isStopping()) {
      _agent->write(report);
    }

  }

  return ret;
}

bool Supervision::updateSnapshot() {

  if (_agent == nullptr || this->isStopping()) {
    return false;
  }
  _snapshot = _agent->readDB().get(_agencyPrefix);
  return true;
}

bool Supervision::doChecks(bool timedout) {

  checkDBServers();
  checkCoordinators();
  return true;

}

void Supervision::run() {

  CONDITION_LOCKER(guard, _cv);
  TRI_ASSERT(_agent != nullptr);
  bool timedout = false;

  while (!this->isStopping()) {

    // Get agency prefix after cluster init
    if (_jobId == 0) {
      // We need the agency prefix to work, but it is only initialized by
      // some other server in the cluster. Since the supervision does not
      // make sense at all without other ArangoDB servers, we wait pretty
      // long here before giving up:
      if (!updateAgencyPrefix(1000, 1)) {
        LOG_TOPIC(ERR, Logger::AGENCY)
          << "Cannot get prefix from Agency. Stopping supervision for good.";
        break;
      }
    }

    // Get bunch of job IDs from agency for future jobs
    if (_jobId == 0 || _jobId == _jobIdMax) {
      getUniqueIds();  // cannot fail but only hang
    }

    // Do nothing unless leader
    if (_agent->leading()) {
      timedout = _cv.wait(_frequency * 1000000);  // quarter second
    } else {
      _cv.wait();
    }

    // Do supervision
    updateSnapshot();
    doChecks(timedout);
    workJobs();

  }

}

void Supervision::workJobs() {

  Node::Children const& todos = _snapshot(toDoPrefix).children();
  Node::Children const& pends = _snapshot(pendingPrefix).children();

  if (!todos.empty()) {
    for (auto const& todoEnt : todos) {
      Node const& job = *todoEnt.second;

      try {
        std::string jobType = job("type").getString(),
          jobId = job("jobId").getString(),
          creator = job("creator").getString();
        if (jobType == "failedServer") {
          FailedServer fs(_snapshot, _agent, jobId, creator, _agencyPrefix);
        } else if (jobType == "cleanOutServer") {
          CleanOutServer cos(_snapshot, _agent, jobId, creator, _agencyPrefix);
        }
      } catch (std::exception const&) {}
    }
  }

  if (!pends.empty()) {
    for (auto const& pendEnt : pends) {
      Node const& job = *pendEnt.second;

      try {
        std::string jobType = job("type").getString(),
          jobId = job("jobId").getString(),
          creator = job("creator").getString();
        if (jobType == "failedServer") {
          FailedServer fs(_snapshot, _agent, jobId, creator, _agencyPrefix);
        } else if (jobType == "cleanOutServer") {
          CleanOutServer cos(_snapshot, _agent, jobId, creator, _agencyPrefix);
        }
      } catch (std::exception const&) {}
    }
  }

}

// Start thread
bool Supervision::start() {
  Thread::start();
  return true;
}

// Start thread with agent
bool Supervision::start(Agent* agent) {
  _agent = agent;
  _frequency = static_cast<long>(_agent->config().supervisionFrequency);

  return start();
}

// Get agency prefix fron agency
bool Supervision::updateAgencyPrefix (size_t nTries, int intervalSec) {

  // Try nTries to get agency's prefix in intervals
  while (!this->isStopping()) {
    _snapshot = _agent->readDB().get("/");
    if (_snapshot.children().size() > 0) {
      _agencyPrefix = std::string("/") + _snapshot.children().begin()->first;
      LOG_TOPIC(DEBUG, Logger::AGENCY) << "Agency prefix is " << _agencyPrefix;
      return true;
    }
    std::this_thread::sleep_for (std::chrono::seconds(intervalSec));
  }

  // Stand-alone agency
  return false;

}

static std::string const syncLatest = "/Sync/LatestID";
// Get bunch of cluster's unique ids from agency
void Supervision::getUniqueIds() {
  uint64_t latestId;
  // Run forever, supervision does not make sense before the agency data
  // is initialized by some other server...
  while (!this->isStopping()) {
    try {
      latestId = std::stoul(
          _agent->readDB().get(_agencyPrefix + "/Sync/LatestID").slice().toJson());
    } catch (std::exception const&) {
      std::this_thread::sleep_for (std::chrono::seconds(1));
      continue;
    }

    Builder uniq;
    uniq.openArray();
    uniq.openObject();
    uniq.add(_agencyPrefix + syncLatest, VPackValue(latestId + 100000)); // new
    uniq.close();
    uniq.openObject();
    uniq.add(_agencyPrefix + syncLatest, VPackValue(latestId));      // precond
    uniq.close();
    uniq.close();

    auto result = transact(_agent, uniq);
    if (result.indices[0]) {
      _agent->waitFor(result.indices[0]);
      _jobId = latestId;
      _jobIdMax = latestId + 100000;
      return;
    }
  }
}

void Supervision::updateFromAgency() {
  auto const& jobsPending =
      _snapshot("/Supervision/Jobs/Pending").children();

  for (auto const& jobent : jobsPending) {
    auto const& job = *(jobent.second);

    LOG_TOPIC(WARN, Logger::AGENCY)
      << job.name() << " " << job("failed").toJson() << job("");
  }
}

void Supervision::beginShutdown() {
  // Personal hygiene
  Thread::beginShutdown();
}

Store const& Supervision::store() const {
  return _agent->readDB();
}
<|endoftext|>


<commit_before><commit_msg>WaE: implicit conversion from bool to 'int'<commit_after><|endoftext|>


<commit_before>#include "SelectLayer.h"

SelectLayer::SelectLayer()
{

}
SelectLayer::~SelectLayer()
{

}<commit_msg>OurClass<commit_after>#include "SelectLayer.h"

SelectLayer::SelectLayer()
{

}

SelectLayer::~SelectLayer()
{

}<|endoftext|>


<commit_before>/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * StringUtils.cpp
 * Random String functions.
 * Copyright (C) 2005-2008 Simon Newton
 */

#include <errno.h>
#include <stdint.h>
#include <stdlib.h>
#include <algorithm>
#include <functional>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include "ola/StringUtils.h"

namespace ola {

using std::string;
using std::stringstream;
using std::vector;

/*
 * Split a string on delimtiers. If two delimiters appear next to each other an
 * empty string is added to the output list.
 * @param input the string to split
 * @param tokens the vector to store the result in
 * @param delimiters what to split on
 */
void StringSplit(const string &input,
                 vector<string> &tokens,
                 const string &delimiters) {
  string::size_type start_offset = 0;
  string::size_type end_offset = 0;

  while (1) {
    end_offset = input.find_first_of(delimiters, start_offset);
    if (end_offset == string::npos) {
      tokens.push_back(input.substr(start_offset, input.size() - start_offset));
      return;
    }
    tokens.push_back(input.substr(start_offset, end_offset - start_offset));
    start_offset = end_offset + 1 > input.size() ? string::npos :
                   end_offset + 1;
  }
}

/*
 * Trim leading and trailing whitespace from a string.
 * @param the string to trim
 */
void StringTrim(std::string *input) {
  string characters_to_trim = " \n\r\t";
  string::size_type start = input->find_first_not_of(characters_to_trim);
  string::size_type end = input->find_last_not_of(characters_to_trim);

  if (start == string::npos)
    input->clear();
  else
    *input = input->substr(start, end - start + 1);
}

/**
 * Take care of any NULLs embedded in the string
 * @param the string to shorten
 */
void ShortenString(string *input) {
  size_t index = input->find(static_cast<char>(0));
  if (index != string::npos)
    input->erase(index);
}

/*
 * Convert an int to a string.
 * @param i the int to convert
 * @return the string representation of the int
 */
string IntToString(int i) {
  stringstream str;
  str << i;
  return str.str();
}

/*
 * Convert an int to a string.
 * @param i the int to convert
 * @return the string representation of the int
 */
string IntToString(unsigned int i) {
  stringstream str;
  str << i;
  return str.str();
}

/*
 * Convert a string to a unsigned int.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, unsigned int *output) {
  if (value.empty())
    return false;
  char *end_ptr;
  errno = 0;
  long l = strtol(value.data(), &end_ptr, 10);
  if (l < 0 || (l == 0 && errno != 0))
    return false;
  if (value == end_ptr)
    return false;
  if (l > UINT32_MAX)
    return false;
  *output = static_cast<unsigned int>(l);
  return true;
}

/*
 * Convert a string to a uint16_t.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, uint16_t *output) {
  unsigned int v;
  if (!StringToInt(value, &v))
    return false;
  if (v > 0xffff)
    return false;
  *output = static_cast<uint16_t>(v);
  return true;
}

/*
 * Convert a string to a uint8_t.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, uint8_t *output) {
  unsigned int v;
  if (!StringToInt(value, &v))
    return false;
  if (v > 0xff)
    return false;
  *output = static_cast<uint8_t>(v);
  return true;
}

/*
 * Convert a string to a int.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, int *output) {
  if (value.empty())
    return false;
  char *end_ptr;
  errno = 0;
  long l = strtol(value.data(), &end_ptr, 10);
  if (l == 0 && errno != 0)
    return false;
  if (value == end_ptr)
    return false;
  if (l < INT32_MIN || l > INT32_MAX)
    return false;
  *output = static_cast<unsigned int>(l);
  return true;
}

/*
 * Convert a string to a int16_t.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, int16_t *output) {
  int v;
  if (!StringToInt(value, &v))
    return false;
  if (v < INT16_MIN || v > INT16_MAX)
    return false;
  *output = static_cast<int16_t>(v);
  return true;
}

/*
 * Convert a string to a int16_t.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, int8_t *output) {
  int v;
  if (!StringToInt(value, &v))
    return false;
  if (v < INT8_MIN || v > INT8_MAX)
    return false;
  *output = static_cast<int8_t>(v);
  return true;
}

/*
 * Escape \
 */
void Escape(string *original) {
  for (string::iterator iter = original->begin(); iter != original->end();
      ++iter) {
    switch (*iter) {
      case '"':
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\\':
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '/':
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\b':
        *iter = 'b';
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\f':
        *iter = 'f';
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\n':
        *iter = 'n';
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\r':
        *iter = 'r';
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\t':
        *iter = 't';
        iter = original->insert(iter, '\\');
        iter++;
        break;
      default:
        break;
    }
  }
}

/**
 * Escape a string, returning a copy
 */
string EscapeString(const string &original) {
  string result = original;
  Escape(&result);
  return result;
}

/*
 * Convert a hex string to a unsigned int
 */
bool HexStringToUInt(const string &value, unsigned int *output) {
  if (value.empty())
    return false;

  size_t found = value.find_first_not_of("ABCDEFabcdef0123456789");
  if (found != string::npos)
    return false;
  *output = strtoul(value.data(), NULL, 16);
  return true;
}

/*
 * Convert a hex string to a uint16_t
 */
bool HexStringToUInt16(const string &value, uint16_t *output) {
  unsigned int v;
  if (!HexStringToUInt(value, &v))
    return false;
  if (v > 0xffff)
    return false;
  *output = static_cast<uint16_t>(v);
  return true;
}

/*
 * Return a lower case version of this string
 */
void ToLower(string *s) {
  std::transform(s->begin(), s->end(), s->begin(),
      std::ptr_fun<int, int>(std::tolower));
}
}  // ola
<commit_msg> * utils: fix the linux build<commit_after>/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * StringUtils.cpp
 * Random String functions.
 * Copyright (C) 2005-2008 Simon Newton
 */

#define __STDC_LIMIT_MACROS  // for UINT8_MAX & friends
#include <errno.h>
#include <stdint.h>
#include <stdlib.h>
#include <algorithm>
#include <functional>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include "ola/StringUtils.h"

namespace ola {

using std::string;
using std::stringstream;
using std::vector;

/*
 * Split a string on delimtiers. If two delimiters appear next to each other an
 * empty string is added to the output list.
 * @param input the string to split
 * @param tokens the vector to store the result in
 * @param delimiters what to split on
 */
void StringSplit(const string &input,
                 vector<string> &tokens,
                 const string &delimiters) {
  string::size_type start_offset = 0;
  string::size_type end_offset = 0;

  while (1) {
    end_offset = input.find_first_of(delimiters, start_offset);
    if (end_offset == string::npos) {
      tokens.push_back(input.substr(start_offset, input.size() - start_offset));
      return;
    }
    tokens.push_back(input.substr(start_offset, end_offset - start_offset));
    start_offset = end_offset + 1 > input.size() ? string::npos :
                   end_offset + 1;
  }
}

/*
 * Trim leading and trailing whitespace from a string.
 * @param the string to trim
 */
void StringTrim(std::string *input) {
  string characters_to_trim = " \n\r\t";
  string::size_type start = input->find_first_not_of(characters_to_trim);
  string::size_type end = input->find_last_not_of(characters_to_trim);

  if (start == string::npos)
    input->clear();
  else
    *input = input->substr(start, end - start + 1);
}

/**
 * Take care of any NULLs embedded in the string
 * @param the string to shorten
 */
void ShortenString(string *input) {
  size_t index = input->find(static_cast<char>(0));
  if (index != string::npos)
    input->erase(index);
}

/*
 * Convert an int to a string.
 * @param i the int to convert
 * @return the string representation of the int
 */
string IntToString(int i) {
  stringstream str;
  str << i;
  return str.str();
}

/*
 * Convert an int to a string.
 * @param i the int to convert
 * @return the string representation of the int
 */
string IntToString(unsigned int i) {
  stringstream str;
  str << i;
  return str.str();
}

/*
 * Convert a string to a unsigned int.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, unsigned int *output) {
  if (value.empty())
    return false;
  char *end_ptr;
  errno = 0;
  long l = strtol(value.data(), &end_ptr, 10);
  if (l < 0 || (l == 0 && errno != 0))
    return false;
  if (value == end_ptr)
    return false;
  if (l > static_cast<long>(UINT32_MAX))
    return false;
  *output = static_cast<unsigned int>(l);
  return true;
}

/*
 * Convert a string to a uint16_t.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, uint16_t *output) {
  unsigned int v;
  if (!StringToInt(value, &v))
    return false;
  if (v > 0xffff)
    return false;
  *output = static_cast<uint16_t>(v);
  return true;
}

/*
 * Convert a string to a uint8_t.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, uint8_t *output) {
  unsigned int v;
  if (!StringToInt(value, &v))
    return false;
  if (v > 0xff)
    return false;
  *output = static_cast<uint8_t>(v);
  return true;
}

/*
 * Convert a string to a int.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, int *output) {
  if (value.empty())
    return false;
  char *end_ptr;
  errno = 0;
  long l = strtol(value.data(), &end_ptr, 10);
  if (l == 0 && errno != 0)
    return false;
  if (value == end_ptr)
    return false;
  if (l < INT32_MIN || l > INT32_MAX)
    return false;
  *output = static_cast<unsigned int>(l);
  return true;
}

/*
 * Convert a string to a int16_t.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, int16_t *output) {
  int v;
  if (!StringToInt(value, &v))
    return false;
  if (v < INT16_MIN || v > INT16_MAX)
    return false;
  *output = static_cast<int16_t>(v);
  return true;
}

/*
 * Convert a string to a int16_t.
 * @returns true if sucessfull, false otherwise
 */
bool StringToInt(const string &value, int8_t *output) {
  int v;
  if (!StringToInt(value, &v))
    return false;
  if (v < INT8_MIN || v > INT8_MAX)
    return false;
  *output = static_cast<int8_t>(v);
  return true;
}

/*
 * Escape \
 */
void Escape(string *original) {
  for (string::iterator iter = original->begin(); iter != original->end();
      ++iter) {
    switch (*iter) {
      case '"':
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\\':
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '/':
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\b':
        *iter = 'b';
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\f':
        *iter = 'f';
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\n':
        *iter = 'n';
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\r':
        *iter = 'r';
        iter = original->insert(iter, '\\');
        iter++;
        break;
      case '\t':
        *iter = 't';
        iter = original->insert(iter, '\\');
        iter++;
        break;
      default:
        break;
    }
  }
}

/**
 * Escape a string, returning a copy
 */
string EscapeString(const string &original) {
  string result = original;
  Escape(&result);
  return result;
}

/*
 * Convert a hex string to a unsigned int
 */
bool HexStringToUInt(const string &value, unsigned int *output) {
  if (value.empty())
    return false;

  size_t found = value.find_first_not_of("ABCDEFabcdef0123456789");
  if (found != string::npos)
    return false;
  *output = strtoul(value.data(), NULL, 16);
  return true;
}

/*
 * Convert a hex string to a uint16_t
 */
bool HexStringToUInt16(const string &value, uint16_t *output) {
  unsigned int v;
  if (!HexStringToUInt(value, &v))
    return false;
  if (v > 0xffff)
    return false;
  *output = static_cast<uint16_t>(v);
  return true;
}

/*
 * Return a lower case version of this string
 */
void ToLower(string *s) {
  std::transform(s->begin(), s->end(), s->begin(),
      std::ptr_fun<int, int>(std::tolower));
}
}  // ola
<|endoftext|>


<commit_before><commit_msg>Make it clear that sal/log.hxx functionality may change again.<commit_after><|endoftext|>


<commit_before>/*=========================================================================

  Program:   Monteverdi2
  Language:  C++

  Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
  See Copyright.txt for details.

  Monteverdi2 is distributed under the CeCILL licence version 2. See
  Licence_CeCILL_V2-en.txt or
  http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt for more details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

//
// Configuration include.
//// Included at first position before any other ones.
#include "ConfigureMonteverdi2.h"

/*****************************************************************************/
/* INCLUDE SECTION                                                           */

//
// Qt includes (sorted by alphabetic order)
//// Must be included before system/custom includes.

//
// System includes (sorted by alphabetic order)

//
// ITK includes (sorted by alphabetic order)

//
// OTB includes (sorted by alphabetic order)

//
// Monteverdi includes (sorted by alphabetic order)
#include "mvdCatalogueApplication.h"
#include "mvdMainWindow.h"

/*****************************************************************************/
/* FUNCTIONS DECLARATION                                                     */

/*****************************************************************************/
/* MAIN                                                                      */

int
main( int argc, char* argv[] )
{
  QApplication qtApp( argc, argv );

  // 1. Initialize application and sync settings.
  mvd::CatalogueApplication application( &qtApp );
  application.Initialize();

  // 2. Initialize main-window (UI).
  mvd::MainWindow mainWindow;
  mainWindow.Initialize();

  // 3. Initialize cache directory.
  try
    {
    mainWindow.SetupCacheDir();
    }
  catch( std::exception& exc )
    {
    QMessageBox::critical(
      &mainWindow,
      QCoreApplication::translate(
	PROJECT_NAME,
	PROJECT_NAME " - Critical error!"
      ),
      QCoreApplication::translate(
	PROJECT_NAME,
	"Error when creating repository cache-directory:\n"
	"%1\n"
	"Application will exit!"
      )
      .arg( exc.what() )
    );

    return -1;
    }

  // 4. Initialize database.
  mvd::CountType nb = application.OpenDatabase();
  if( nb > 0 )
    {
    QMessageBox::StandardButton button =
      QMessageBox::warning(
	&mainWindow,
	QCoreApplication::translate(
	  PROJECT_NAME,
	  PROJECT_NAME " " Monteverdi2_VERSION_STRING " - Warning! "
	),
	QCoreApplication::translate(
	  PROJECT_NAME,
	  "There are %1 outdated dataset(s) in cache-directory. "
	  "Please remove cache-directory and restart "
	  PROJECT_NAME " " Monteverdi2_VERSION_STRING ".\n"
	  "Do you want to quit now ?"
	).arg( nb ),
	QMessageBox::Yes | QMessageBox::No,
	QMessageBox::Yes
      );

    if( button==QMessageBox::Yes )
      {
      return -2;
      }
    }

  // 5. Show window.
#if defined( _DEBUG )
  // Usefull when developping/debugging to avoid overlapping other windows.
  mainWindow.show();
#else
  // TODO: Correctly manage main-window state via application settings.
  mainWindow.showMaximized();
#endif

  // 6. Let's go: run the application and return exit code.
  return QCoreApplication::instance()->exec();
}

/*****************************************************************************/
/* FUNCTIONS IMPLEMENTATION                                                  */
<commit_msg>ENH: Added newlines to confirm quit when outdated dataset models dialog.<commit_after>/*=========================================================================

  Program:   Monteverdi2
  Language:  C++

  Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
  See Copyright.txt for details.

  Monteverdi2 is distributed under the CeCILL licence version 2. See
  Licence_CeCILL_V2-en.txt or
  http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt for more details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

//
// Configuration include.
//// Included at first position before any other ones.
#include "ConfigureMonteverdi2.h"

/*****************************************************************************/
/* INCLUDE SECTION                                                           */

//
// Qt includes (sorted by alphabetic order)
//// Must be included before system/custom includes.

//
// System includes (sorted by alphabetic order)

//
// ITK includes (sorted by alphabetic order)

//
// OTB includes (sorted by alphabetic order)

//
// Monteverdi includes (sorted by alphabetic order)
#include "mvdCatalogueApplication.h"
#include "mvdMainWindow.h"

/*****************************************************************************/
/* FUNCTIONS DECLARATION                                                     */

/*****************************************************************************/
/* MAIN                                                                      */

int
main( int argc, char* argv[] )
{
  QApplication qtApp( argc, argv );

  // 1. Initialize application and sync settings.
  mvd::CatalogueApplication application( &qtApp );
  application.Initialize();

  // 2. Initialize main-window (UI).
  mvd::MainWindow mainWindow;
  mainWindow.Initialize();

  // 3. Initialize cache directory.
  try
    {
    mainWindow.SetupCacheDir();
    }
  catch( std::exception& exc )
    {
    QMessageBox::critical(
      &mainWindow,
      QCoreApplication::translate(
	PROJECT_NAME,
	PROJECT_NAME " - Critical error!"
      ),
      QCoreApplication::translate(
	PROJECT_NAME,
	"Error when creating repository cache-directory:\n"
	"%1\n"
	"Application will exit!"
      )
      .arg( exc.what() )
    );

    return -1;
    }

  // 4. Initialize database.
  mvd::CountType nb = application.OpenDatabase();
  if( nb > 0 )
    {
    QMessageBox::StandardButton button =
      QMessageBox::warning(
	&mainWindow,
	QCoreApplication::translate(
	  PROJECT_NAME,
	  PROJECT_NAME " " Monteverdi2_VERSION_STRING " - Warning! "
	),
	QCoreApplication::translate(
	  PROJECT_NAME,
	  "There are %1 outdated dataset(s) in cache-directory.\n\n"
	  "Please remove cache-directory and restart "
	  PROJECT_NAME " " Monteverdi2_VERSION_STRING ".\n\n"
	  "Do you want to quit now ?"
	).arg( nb ),
	QMessageBox::Yes | QMessageBox::No,
	QMessageBox::Yes
      );

    if( button==QMessageBox::Yes )
      {
      return -2;
      }
    }

  // 5. Show window.
#if defined( _DEBUG )
  // Usefull when developping/debugging to avoid overlapping other windows.
  mainWindow.show();
#else
  // TODO: Correctly manage main-window state via application settings.
  mainWindow.showMaximized();
#endif

  // 6. Let's go: run the application and return exit code.
  return QCoreApplication::instance()->exec();
}

/*****************************************************************************/
/* FUNCTIONS IMPLEMENTATION                                                  */
<|endoftext|>


<commit_before>#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <string>

extern "C" {
    void* cvopen(const char* path);
    void cvclose(void* context);
    int cvcapture(void* context, void* buffer, size_t width, size_t height);
}

#include "opencv2/opencv.hpp"

void* cvopen(const char* path) {
    cv::VideoCapture* context;
    if (path) {
        context = new cv::VideoCapture(std::string(path));
    } else {
        context = new cv::VideoCapture(0);
    }
    return reinterpret_cast<void*>(context);
}

void cvclose(void* context) {
    cv::VideoCapture* cap = reinterpret_cast<cv::VideoCapture*>(context);
    delete cap;
}

int cvcapture(void* context, void* buffer, size_t width, size_t height) {
    cv::VideoCapture* cap = reinterpret_cast<cv::VideoCapture*>(context);
    cap->set(CV_CAP_PROP_FRAME_WIDTH, width);
    cap->set(CV_CAP_PROP_FRAME_HEIGHT, height);
    cap->set(CV_CAP_PROP_FOURCC, CV_FOURCC('B', 'G', 'R', 3));

    if (cap->get(CV_CAP_PROP_FRAME_WIDTH) != (double)width) {
        fprintf(stderr, "Incorrect width set on cap: %f\n", cap->get(CV_CAP_PROP_FRAME_WIDTH));
        return 0;
    }

    if (cap->get(CV_CAP_PROP_FRAME_HEIGHT) != (double)height) {
        fprintf(stderr, "Incorrect height set on cap: %f\n", cap->get(CV_CAP_PROP_FRAME_HEIGHT));
        return 0;
    }

    cv::Mat colour_image, greyscale_image, denoised_image, thresholded_image;

    if (!cap->isOpened()) {
        return 0;
    }

    (*cap) >> colour_image;
    cv::cvtColor(colour_image, greyscale_image, cv::COLOR_BGR2GRAY);
    cv::medianBlur(greyscale_image, denoised_image, 3);
    if (!denoised_image.isContinuous()) {
        return 0;
    }
    int died_horribly = 0;
    if (denoised_image.size().width != width) {
        fprintf(
            stderr,
            "Width mismatch: %d expected, %d actual\n",
            width,
            denoised_image.size().width
        );
        died_horribly = 1;
    }
    if (denoised_image.size().height != height) {
        fprintf(
            stderr,
            "Height mismatch: %d expected, %d actual\n",
            height,
            denoised_image.size().height
        );
        died_horribly = 1;
    }
    if (died_horribly) {
        return 0;
    }
    size_t kernel_size = (width < height) ? width : height;
    kernel_size /= 2;
    kernel_size |= 1;
    cv::adaptiveThreshold(
        denoised_image,
        thresholded_image,
        255.0,
        CV_ADAPTIVE_THRESH_MEAN_C,
        CV_THRESH_BINARY,
        kernel_size,
        0.0
    );
    memcpy(
        buffer,
        thresholded_image.ptr(),
        width * height
    );
    return 1;
}
<commit_msg>Make this return NULL if the device cannot be opened<commit_after>#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <string>

extern "C" {
    void* cvopen(const char* path);
    void cvclose(void* context);
    int cvcapture(void* context, void* buffer, size_t width, size_t height);
}

#include "opencv2/opencv.hpp"

void* cvopen(const char* path) {
    cv::VideoCapture* context;
    if (path) {
        context = new cv::VideoCapture(std::string(path));
    } else {
        context = new cv::VideoCapture(0);
    }
    if (!context->isOpened()) {
        delete context;
        return NULL;
    }
    return reinterpret_cast<void*>(context);
}

void cvclose(void* context) {
    cv::VideoCapture* cap = reinterpret_cast<cv::VideoCapture*>(context);
    delete cap;
}

int cvcapture(void* context, void* buffer, size_t width, size_t height) {
    cv::VideoCapture* cap = reinterpret_cast<cv::VideoCapture*>(context);
    cap->set(CV_CAP_PROP_FRAME_WIDTH, width);
    cap->set(CV_CAP_PROP_FRAME_HEIGHT, height);
    cap->set(CV_CAP_PROP_FOURCC, CV_FOURCC('B', 'G', 'R', 3));

    if (cap->get(CV_CAP_PROP_FRAME_WIDTH) != (double)width) {
        fprintf(stderr, "Incorrect width set on cap: %f\n", cap->get(CV_CAP_PROP_FRAME_WIDTH));
        return 0;
    }

    if (cap->get(CV_CAP_PROP_FRAME_HEIGHT) != (double)height) {
        fprintf(stderr, "Incorrect height set on cap: %f\n", cap->get(CV_CAP_PROP_FRAME_HEIGHT));
        return 0;
    }

    cv::Mat colour_image, greyscale_image, denoised_image, thresholded_image;

    if (!cap->isOpened()) {
        return 0;
    }

    (*cap) >> colour_image;
    cv::cvtColor(colour_image, greyscale_image, cv::COLOR_BGR2GRAY);
    cv::medianBlur(greyscale_image, denoised_image, 3);
    if (!denoised_image.isContinuous()) {
        return 0;
    }
    int died_horribly = 0;
    if (denoised_image.size().width != width) {
        fprintf(
            stderr,
            "Width mismatch: %d expected, %d actual\n",
            width,
            denoised_image.size().width
        );
        died_horribly = 1;
    }
    if (denoised_image.size().height != height) {
        fprintf(
            stderr,
            "Height mismatch: %d expected, %d actual\n",
            height,
            denoised_image.size().height
        );
        died_horribly = 1;
    }
    if (died_horribly) {
        return 0;
    }
    size_t kernel_size = (width < height) ? width : height;
    kernel_size /= 2;
    kernel_size |= 1;
    cv::adaptiveThreshold(
        denoised_image,
        thresholded_image,
        255.0,
        CV_ADAPTIVE_THRESH_MEAN_C,
        CV_THRESH_BINARY,
        kernel_size,
        0.0
    );
    memcpy(
        buffer,
        thresholded_image.ptr(),
        width * height
    );
    return 1;
}
<|endoftext|>


<commit_before>// Copyright (c) 2016, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/pinocchio/urdf/util.hh>

#include <urdf_parser/urdf_parser.h>

#include <pinocchio/parsers/utils.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/multibody/geometry.hpp>
#include <pinocchio/algorithm/geometry.hpp>
#include <pinocchio/parsers/srdf.hpp>

#include <hpp/util/debug.hh>

#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/humanoid-robot.hh>

namespace hpp {
  namespace pinocchio {
    namespace urdf {
      using se3::FrameIndex;

      namespace {
#ifdef HPP_DEBUG
        const bool verbose = true;
#else
        const bool verbose = false;
#endif

        JointPtr_t findSpecialJoint (const HumanoidRobotPtr_t& robot, const std::string& linkName)
        {
          return robot->getJointByBodyName (linkName);
        }

        void setSpecialJoints (const HumanoidRobotPtr_t& robot, std::string prefix)
        {
          if (!prefix.empty() && *prefix.rbegin() != '/') prefix += "/";
          try {
            robot->waist (robot->getJointByName(prefix + "root_joint"));
          } catch (const std::exception&) {
            hppDout (notice, "No waist joint found");
          }
          try {
            robot->chest (findSpecialJoint (robot, prefix + "chest"));
          } catch (const std::exception&) {
            hppDout (notice, "No chest joint found");
          }
          try {
            robot->leftWrist (findSpecialJoint (robot, prefix + "l_wrist"));
          } catch (const std::exception&) {
            hppDout (notice, "No left wrist joint found");
          }
          try {
            robot->rightWrist (findSpecialJoint (robot, prefix + "r_wrist"));
          } catch (const std::exception&) {
            hppDout (notice, "No right wrist joint found");
          }
          try {
            robot->leftAnkle (findSpecialJoint (robot, prefix + "l_ankle"));
          } catch (const std::exception&) {
            hppDout (notice, "No left ankle joint found");
          }
          try {
            robot->rightAnkle (findSpecialJoint (robot, prefix + "r_ankle"));
          } catch (const std::exception&) {
            hppDout (notice, "No right ankle joint found");
          }
          try {
            robot->gazeJoint (findSpecialJoint (robot, prefix + "gaze"));
          } catch (const std::exception&) {
            hppDout (notice, "No gaze joint found");
          }
        }

        void fillGaze (const HumanoidRobotPtr_t robot)
        {
          vector3_t dir, origin;
          // Gaze direction is defined by the gaze joint local
          // orientation.
          dir[0] = 1;
          dir[1] = 0;
          dir[2] = 0;
          // Gaze position should is defined by the gaze joint local
          // origin.
          origin[0] = 0;
          origin[1] = 0;
          origin[2] = 0;
          robot->gaze (dir, origin);
        }

        se3::JointModelVariant buildJoint (const std::string& type)
        {
          if (type == "freeflyer")        return se3::JointModelFreeFlyer();
          else if (type == "planar")      return se3::JointModelPlanar();
          else if (type == "prismatic_x") return se3::JointModelPrismatic<0>();
          else if (type == "prismatic_y") return se3::JointModelPrismatic<1>();
          else                          throw  std::invalid_argument
            ("Root joint type is currently not available.");
        }

        void setPrefix (const std::string& prefix,
            Model& model, GeomModel& geomModel,
            const JointIndex& idFirstJoint,
            const FrameIndex& idFirstFrame)
        {
          for (JointIndex i = idFirstJoint; i < model.joints.size(); ++i) {
            model.names[i] = prefix + model.names[i];
          }
          for (FrameIndex i = idFirstFrame; i < model.frames.size(); ++i) {
            se3::Frame& f = model.frames[i];
            f.name = prefix + f.name;
          }
          BOOST_FOREACH(se3::GeometryObject& go, geomModel.geometryObjects) {
            go.name = prefix + go.name;
          }
        }

        void setRootJointBounds(Model& model,
            const JointIndex& rtIdx,
            const std::string& rootType)
        {
          value_type b = std::numeric_limits<value_type>::infinity();
          if (rootType == "freeflyer") {
            const std::size_t idx = model.joints[rtIdx].idx_q();
            model.upperPositionLimit.segment<3>(idx).setConstant(+b);
            model.lowerPositionLimit.segment<3>(idx).setConstant(-b);
            // Quaternion bounds
            b = 1.01;
            const size_type quat_idx = idx + 3;
            model.upperPositionLimit.segment<4>(quat_idx).setConstant(+b);
            model.lowerPositionLimit.segment<4>(quat_idx).setConstant(-b);
          } else if (rootType == "planar") {
            const std::size_t idx = model.joints[rtIdx].idx_q();
            model.upperPositionLimit.segment<2>(idx).setConstant(+b);
            model.lowerPositionLimit.segment<2>(idx).setConstant(-b);
            // Unit complex bounds
            b = 1.01;
            const size_type cplx_idx = idx + 2;
            model.upperPositionLimit.segment<2>(cplx_idx).setConstant(+b);
            model.lowerPositionLimit.segment<2>(cplx_idx).setConstant(-b);
          }
        }

        std::string makeModelPath (const std::string& package,
                                   const std::string& type,
                                   const std::string& modelName,
                                   const std::string& suffix = "")
        {
          std::stringstream ss;
          ss << "package://" << package << "/" << type << "/" << modelName << suffix << "." << type;
          return ss.str();
        }

        template <bool XmlString>
        void _removeCollisionPairs (
            const Model& model,
            GeomModel& geomModel,
            const std::string& srdf,
            bool verbose)
        {
          se3::srdf::removeCollisionPairsFromSrdfString
            (model, geomModel, srdf, verbose);
        }

        template <>
        void _removeCollisionPairs<false> (
            const Model& model,
            GeomModel& geomModel,
            const std::string& srdf,
            bool verbose)
        {
          se3::srdf::removeCollisionPairsFromSrdf
            (model, geomModel, srdf, verbose);
        }

        template <bool srdfAsXmlString, typename InType>
        void _loadModel (const DevicePtr_t& robot,
                        const JointIndex&  baseJoint,
                        std::string prefix,
                        const std::string& rootType,
                        const InType& urdf,
                        const std::string& srdf)
        {
          if (baseJoint != 0)
            throw std::invalid_argument ("Only appending robots at the world is supported.");

          Model& model = robot->model();
          const JointIndex idFirstJoint = model.joints.size();
          const FrameIndex idFirstFrame = model.frames.size();
          if (rootType == "anchor")
            se3::urdf::buildModel(urdf, model, verbose);
          else
            se3::urdf::buildModel(urdf, buildJoint(rootType), model, verbose);
          robot->createData();

          hppDout (notice, "Finished parsing URDF file.");

          GeomModel geomModel;

          std::vector<std::string> baseDirs = se3::rosPaths();
          se3::urdf::buildGeom(model, urdf, se3::COLLISION, geomModel, baseDirs);
          geomModel.addAllCollisionPairs();

          if (!srdf.empty()) {
            _removeCollisionPairs<srdfAsXmlString>
              (model, geomModel, srdf, verbose);
          }

          if (!prefix.empty()) {
            if (*prefix.rbegin() != '/') prefix += "/";
            setPrefix(prefix, model, geomModel, idFirstJoint, idFirstFrame);
          }

          // Update root joint bounds
          assert((rootType == "anchor")
              || (model.names[idFirstJoint] == prefix + "root_joint"));
          setRootJointBounds(model, idFirstJoint, rootType);

          se3::appendGeometryModel(robot->geomModel(), geomModel);
          robot->createGeomData();

          hppDout (notice, "Finished parsing SRDF file.");
        }

      }

      void loadRobotModel (const DevicePtr_t& robot,
			   const std::string& rootJointType,
			   const std::string& package,
			   const std::string& modelName,
			   const std::string& urdfSuffix,
                           const std::string& srdfSuffix)
      {
        loadModel (robot, 0, "", rootJointType,
            makeModelPath(package, "urdf", modelName, urdfSuffix),
            makeModelPath(package, "srdf", modelName, srdfSuffix));
      }

      void loadRobotModel (const DevicePtr_t& robot,
                           const JointIndex&  baseJoint,
			   const std::string& prefix,
			   const std::string& rootJointType,
			   const std::string& package,
			   const std::string& modelName,
			   const std::string& urdfSuffix,
                           const std::string& srdfSuffix)
      {
        loadModel (robot, baseJoint,
            (prefix.empty() ? "" : prefix + "/"),
            rootJointType,
            makeModelPath(package, "urdf", modelName, urdfSuffix),
            makeModelPath(package, "srdf", modelName, srdfSuffix));
      }

      void setupHumanoidRobot (const HumanoidRobotPtr_t& robot,
          const std::string& prefix)
      {
	// Look for special joints and attach them to the model.
	setSpecialJoints (robot, prefix);
	// Fill gaze position and direction.
	fillGaze (robot);
      }

      void loadHumanoidModel (const HumanoidRobotPtr_t& robot,
			      const std::string& rootJointType,
			      const std::string& package,
			      const std::string& modelName,
			      const std::string& urdfSuffix,
			      const std::string& srdfSuffix)
      {
        loadRobotModel(robot, rootJointType, package, modelName, urdfSuffix, srdfSuffix);
        setupHumanoidRobot(robot);
      }

      void loadHumanoidModel (const HumanoidRobotPtr_t& robot,
                              const JointIndex&  baseJoint,
			      const std::string& prefix,
			      const std::string& rootJointType,
			      const std::string& package,
			      const std::string& modelName,
			      const std::string& urdfSuffix,
			      const std::string& srdfSuffix)
      {
        loadRobotModel(robot, baseJoint, prefix, rootJointType, package, modelName, urdfSuffix, srdfSuffix);
        setupHumanoidRobot(robot);
      }

      void loadUrdfModel (const DevicePtr_t& robot,
                          const JointIndex&  baseJoint,
                          const std::string& prefix,
			  const std::string& rootJointType,
			  const std::string& package,
			  const std::string& filename)
      {
        loadModel (robot, baseJoint,
            (prefix.empty() ? "" : prefix + "/"),
            rootJointType,
            makeModelPath(package, "urdf", filename), "");
      }

      void loadUrdfModel (const DevicePtr_t& robot,
			  const std::string& rootType,
			  const std::string& package,
			  const std::string& filename)
      {
        loadModel (robot, 0, "", rootType,
            makeModelPath(package, "urdf", filename), "");
      }

        void loadModel (const DevicePtr_t& robot,
                        const JointIndex&  baseJoint,
                        const std::string& prefix,
                        const std::string& rootType,
                        const std::string& urdfPath,
                        const std::string& srdfPath)
        {
          std::vector<std::string> baseDirs = se3::rosPaths();

          std::string urdfFileName = se3::retrieveResourcePath(urdfPath, baseDirs);
          if (urdfFileName == "") {
            throw std::invalid_argument (std::string ("Unable to retrieve ") +
                urdfPath);
          }

          std::string srdfFileName;
          if (!srdfPath.empty()) {
            srdfFileName = se3::retrieveResourcePath(srdfPath, baseDirs);
            if (srdfFileName == "") {
              throw std::invalid_argument (std::string ("Unable to retrieve ") +
                  srdfPath);
            }
          }

          _loadModel <false> (robot, baseJoint, prefix, rootType,
              urdfFileName, srdfFileName);
        }

        void loadModelFromString (const DevicePtr_t& robot,
                                  const JointIndex&  baseJoint,
                                  const std::string& prefix,
                                  const std::string& rootType,
                                  const std::string& urdfString,
                                  const std::string& srdfString)
        {
          ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(urdfString);
          if (!urdfTree) {
            throw std::invalid_argument ("Unable to parse the input URDF string");
          }
          _loadModel <true> (robot, baseJoint, prefix, rootType,
              urdfTree, srdfString);
        }
    } // end of namespace urdf.
  } // end of namespace pinocchio.
} // end of namespace  hpp.
<commit_msg>Make error message more explicit<commit_after>// Copyright (c) 2016, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/pinocchio/urdf/util.hh>

#include <urdf_parser/urdf_parser.h>

#include <pinocchio/parsers/utils.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/multibody/geometry.hpp>
#include <pinocchio/algorithm/geometry.hpp>
#include <pinocchio/parsers/srdf.hpp>

#include <hpp/util/debug.hh>

#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/humanoid-robot.hh>

namespace hpp {
  namespace pinocchio {
    namespace urdf {
      using se3::FrameIndex;

      namespace {
#ifdef HPP_DEBUG
        const bool verbose = true;
#else
        const bool verbose = false;
#endif

        JointPtr_t findSpecialJoint (const HumanoidRobotPtr_t& robot, const std::string& linkName)
        {
          return robot->getJointByBodyName (linkName);
        }

        void setSpecialJoints (const HumanoidRobotPtr_t& robot, std::string prefix)
        {
          if (!prefix.empty() && *prefix.rbegin() != '/') prefix += "/";
          try {
            robot->waist (robot->getJointByName(prefix + "root_joint"));
          } catch (const std::exception&) {
            hppDout (notice, "No waist joint found");
          }
          try {
            robot->chest (findSpecialJoint (robot, prefix + "chest"));
          } catch (const std::exception&) {
            hppDout (notice, "No chest joint found");
          }
          try {
            robot->leftWrist (findSpecialJoint (robot, prefix + "l_wrist"));
          } catch (const std::exception&) {
            hppDout (notice, "No left wrist joint found");
          }
          try {
            robot->rightWrist (findSpecialJoint (robot, prefix + "r_wrist"));
          } catch (const std::exception&) {
            hppDout (notice, "No right wrist joint found");
          }
          try {
            robot->leftAnkle (findSpecialJoint (robot, prefix + "l_ankle"));
          } catch (const std::exception&) {
            hppDout (notice, "No left ankle joint found");
          }
          try {
            robot->rightAnkle (findSpecialJoint (robot, prefix + "r_ankle"));
          } catch (const std::exception&) {
            hppDout (notice, "No right ankle joint found");
          }
          try {
            robot->gazeJoint (findSpecialJoint (robot, prefix + "gaze"));
          } catch (const std::exception&) {
            hppDout (notice, "No gaze joint found");
          }
        }

        void fillGaze (const HumanoidRobotPtr_t robot)
        {
          vector3_t dir, origin;
          // Gaze direction is defined by the gaze joint local
          // orientation.
          dir[0] = 1;
          dir[1] = 0;
          dir[2] = 0;
          // Gaze position should is defined by the gaze joint local
          // origin.
          origin[0] = 0;
          origin[1] = 0;
          origin[2] = 0;
          robot->gaze (dir, origin);
        }

        se3::JointModelVariant buildJoint (const std::string& type)
        {
          if (type == "freeflyer")        return se3::JointModelFreeFlyer();
          else if (type == "planar")      return se3::JointModelPlanar();
          else if (type == "prismatic_x") return se3::JointModelPrismatic<0>();
          else if (type == "prismatic_y") return se3::JointModelPrismatic<1>();
          else                          throw  std::invalid_argument
            ("Root joint type \"" + type + "\" is currently not available.");
        }

        void setPrefix (const std::string& prefix,
            Model& model, GeomModel& geomModel,
            const JointIndex& idFirstJoint,
            const FrameIndex& idFirstFrame)
        {
          for (JointIndex i = idFirstJoint; i < model.joints.size(); ++i) {
            model.names[i] = prefix + model.names[i];
          }
          for (FrameIndex i = idFirstFrame; i < model.frames.size(); ++i) {
            se3::Frame& f = model.frames[i];
            f.name = prefix + f.name;
          }
          BOOST_FOREACH(se3::GeometryObject& go, geomModel.geometryObjects) {
            go.name = prefix + go.name;
          }
        }

        void setRootJointBounds(Model& model,
            const JointIndex& rtIdx,
            const std::string& rootType)
        {
          value_type b = std::numeric_limits<value_type>::infinity();
          if (rootType == "freeflyer") {
            const std::size_t idx = model.joints[rtIdx].idx_q();
            model.upperPositionLimit.segment<3>(idx).setConstant(+b);
            model.lowerPositionLimit.segment<3>(idx).setConstant(-b);
            // Quaternion bounds
            b = 1.01;
            const size_type quat_idx = idx + 3;
            model.upperPositionLimit.segment<4>(quat_idx).setConstant(+b);
            model.lowerPositionLimit.segment<4>(quat_idx).setConstant(-b);
          } else if (rootType == "planar") {
            const std::size_t idx = model.joints[rtIdx].idx_q();
            model.upperPositionLimit.segment<2>(idx).setConstant(+b);
            model.lowerPositionLimit.segment<2>(idx).setConstant(-b);
            // Unit complex bounds
            b = 1.01;
            const size_type cplx_idx = idx + 2;
            model.upperPositionLimit.segment<2>(cplx_idx).setConstant(+b);
            model.lowerPositionLimit.segment<2>(cplx_idx).setConstant(-b);
          }
        }

        std::string makeModelPath (const std::string& package,
                                   const std::string& type,
                                   const std::string& modelName,
                                   const std::string& suffix = "")
        {
          std::stringstream ss;
          ss << "package://" << package << "/" << type << "/" << modelName << suffix << "." << type;
          return ss.str();
        }

        template <bool XmlString>
        void _removeCollisionPairs (
            const Model& model,
            GeomModel& geomModel,
            const std::string& srdf,
            bool verbose)
        {
          se3::srdf::removeCollisionPairsFromSrdfString
            (model, geomModel, srdf, verbose);
        }

        template <>
        void _removeCollisionPairs<false> (
            const Model& model,
            GeomModel& geomModel,
            const std::string& srdf,
            bool verbose)
        {
          se3::srdf::removeCollisionPairsFromSrdf
            (model, geomModel, srdf, verbose);
        }

        template <bool srdfAsXmlString, typename InType>
        void _loadModel (const DevicePtr_t& robot,
                        const JointIndex&  baseJoint,
                        std::string prefix,
                        const std::string& rootType,
                        const InType& urdf,
                        const std::string& srdf)
        {
          if (baseJoint != 0)
            throw std::invalid_argument ("Only appending robots at the world is supported.");

          Model& model = robot->model();
          const JointIndex idFirstJoint = model.joints.size();
          const FrameIndex idFirstFrame = model.frames.size();
          if (rootType == "anchor")
            se3::urdf::buildModel(urdf, model, verbose);
          else
            se3::urdf::buildModel(urdf, buildJoint(rootType), model, verbose);
          robot->createData();

          hppDout (notice, "Finished parsing URDF file.");

          GeomModel geomModel;

          std::vector<std::string> baseDirs = se3::rosPaths();
          se3::urdf::buildGeom(model, urdf, se3::COLLISION, geomModel, baseDirs);
          geomModel.addAllCollisionPairs();

          if (!srdf.empty()) {
            _removeCollisionPairs<srdfAsXmlString>
              (model, geomModel, srdf, verbose);
          }

          if (!prefix.empty()) {
            if (*prefix.rbegin() != '/') prefix += "/";
            setPrefix(prefix, model, geomModel, idFirstJoint, idFirstFrame);
          }

          // Update root joint bounds
          assert((rootType == "anchor")
              || (model.names[idFirstJoint] == prefix + "root_joint"));
          setRootJointBounds(model, idFirstJoint, rootType);

          se3::appendGeometryModel(robot->geomModel(), geomModel);
          robot->createGeomData();

          hppDout (notice, "Finished parsing SRDF file.");
        }

      }

      void loadRobotModel (const DevicePtr_t& robot,
			   const std::string& rootJointType,
			   const std::string& package,
			   const std::string& modelName,
			   const std::string& urdfSuffix,
                           const std::string& srdfSuffix)
      {
        loadModel (robot, 0, "", rootJointType,
            makeModelPath(package, "urdf", modelName, urdfSuffix),
            makeModelPath(package, "srdf", modelName, srdfSuffix));
      }

      void loadRobotModel (const DevicePtr_t& robot,
                           const JointIndex&  baseJoint,
			   const std::string& prefix,
			   const std::string& rootJointType,
			   const std::string& package,
			   const std::string& modelName,
			   const std::string& urdfSuffix,
                           const std::string& srdfSuffix)
      {
        loadModel (robot, baseJoint,
            (prefix.empty() ? "" : prefix + "/"),
            rootJointType,
            makeModelPath(package, "urdf", modelName, urdfSuffix),
            makeModelPath(package, "srdf", modelName, srdfSuffix));
      }

      void setupHumanoidRobot (const HumanoidRobotPtr_t& robot,
          const std::string& prefix)
      {
	// Look for special joints and attach them to the model.
	setSpecialJoints (robot, prefix);
	// Fill gaze position and direction.
	fillGaze (robot);
      }

      void loadHumanoidModel (const HumanoidRobotPtr_t& robot,
			      const std::string& rootJointType,
			      const std::string& package,
			      const std::string& modelName,
			      const std::string& urdfSuffix,
			      const std::string& srdfSuffix)
      {
        loadRobotModel(robot, rootJointType, package, modelName, urdfSuffix, srdfSuffix);
        setupHumanoidRobot(robot);
      }

      void loadHumanoidModel (const HumanoidRobotPtr_t& robot,
                              const JointIndex&  baseJoint,
			      const std::string& prefix,
			      const std::string& rootJointType,
			      const std::string& package,
			      const std::string& modelName,
			      const std::string& urdfSuffix,
			      const std::string& srdfSuffix)
      {
        loadRobotModel(robot, baseJoint, prefix, rootJointType, package, modelName, urdfSuffix, srdfSuffix);
        setupHumanoidRobot(robot);
      }

      void loadUrdfModel (const DevicePtr_t& robot,
                          const JointIndex&  baseJoint,
                          const std::string& prefix,
			  const std::string& rootJointType,
			  const std::string& package,
			  const std::string& filename)
      {
        loadModel (robot, baseJoint,
            (prefix.empty() ? "" : prefix + "/"),
            rootJointType,
            makeModelPath(package, "urdf", filename), "");
      }

      void loadUrdfModel (const DevicePtr_t& robot,
			  const std::string& rootType,
			  const std::string& package,
			  const std::string& filename)
      {
        loadModel (robot, 0, "", rootType,
            makeModelPath(package, "urdf", filename), "");
      }

        void loadModel (const DevicePtr_t& robot,
                        const JointIndex&  baseJoint,
                        const std::string& prefix,
                        const std::string& rootType,
                        const std::string& urdfPath,
                        const std::string& srdfPath)
        {
          std::vector<std::string> baseDirs = se3::rosPaths();

          std::string urdfFileName = se3::retrieveResourcePath(urdfPath, baseDirs);
          if (urdfFileName == "") {
            throw std::invalid_argument (std::string ("Unable to retrieve ") +
                urdfPath);
          }

          std::string srdfFileName;
          if (!srdfPath.empty()) {
            srdfFileName = se3::retrieveResourcePath(srdfPath, baseDirs);
            if (srdfFileName == "") {
              throw std::invalid_argument (std::string ("Unable to retrieve ") +
                  srdfPath);
            }
          }

          _loadModel <false> (robot, baseJoint, prefix, rootType,
              urdfFileName, srdfFileName);
        }

        void loadModelFromString (const DevicePtr_t& robot,
                                  const JointIndex&  baseJoint,
                                  const std::string& prefix,
                                  const std::string& rootType,
                                  const std::string& urdfString,
                                  const std::string& srdfString)
        {
          ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(urdfString);
          if (!urdfTree) {
            throw std::invalid_argument ("Unable to parse the input URDF string");
          }
          _loadModel <true> (robot, baseJoint, prefix, rootType,
              urdfTree, srdfString);
        }
    } // end of namespace urdf.
  } // end of namespace pinocchio.
} // end of namespace  hpp.
<|endoftext|>


<commit_before>// The K3 Runtime Collections Library.
//
// This file contains definitions for the various operations performed on K3 collections, used by
// the generated C++ code. The C++ realizations of K3 Collections will inherit from the
// K3::Collection class, providing a suitable content type.
#ifndef K3_RUNTIME_COLLECTIONS_H
#define K3_RUNTIME_COLLECTIONS_H

#include <functional>
#include <map>
#include <tuple>
#include <vector>

namespace K3 {
    template <typename E>
    class Collection {
        public:
            Collection() {}
            Collection(std::vector<E> v): __data(v) {}
            Collection(const Collection& c): __data(c.__data) {}
            Collection(Collection&& c): __data(c.__data) {}

            // Virtual sub-class constructor.
            template <typename T>
            virtual Collection<T> create(Collection<T>) = 0;

            template <typename T>
            Collection<T> map(std::function<T(E)>);

            template <typename K, typename Z>
            Collection<std::tuple<K, Z>> group_by(std::function<K(E)>, std::function<Z(Z, E)>, Z);
        private:
            std::vector<E> __data;
    };

    template <typename E>
    template <typename T>
    Collection<T> Collection<E>::map(std::function<T(E)> f) {
        std::vector<T> v;
        v.reserve(__data.size());

        for (auto i : __data) {
            v.push_back(f(i));
        }

        return create(Collection(v));
    }

    template <typename E>
    template <typename K, typename Z>
    Collection<std::tuple<K, Z>> Collection<E>::group_by(std::function<K(E)> g, std::function<Z(Z, E)> f, Z z) {
        std::map<K, Z> m;

        for (auto i: __data) {
            K k = g(i);
            if (m.find(k)) {
                m[k] = f(i, m[k]);
            } else {
                m[k] = f(z, i);
            }
        }

        std::vector<std::tuple<K, Z>> v;
        v.reserve(m.count());

        for (auto i: m) {
            v.push_back(std::make_tuple(i.first, i.second));
        }

        return create(Collection(v));
    }
}

#endif
<commit_msg>Use type-alias for data-representation type.<commit_after>// The K3 Runtime Collections Library.
//
// This file contains definitions for the various operations performed on K3 collections, used by
// the generated C++ code. The C++ realizations of K3 Collections will inherit from the
// K3::Collection class, providing a suitable content type.
#ifndef K3_RUNTIME_COLLECTIONS_H
#define K3_RUNTIME_COLLECTIONS_H

#include <functional>
#include <map>
#include <tuple>
#include <vector>

namespace K3 {
    template <typename E> using chunk = std::vector<E>;

    template <typename E>
    class Collection {
        public:
            Collection() {}
            Collection(const chunk<E>& v): __data(v) {}
            Collection(const Collection& c): __data(c.__data) {}
            Collection(Collection&& c): __data(c.__data) {}

            template <typename T>
            Collection<T> map(std::function<T(E)>);

            template <typename K, typename Z>
            Collection<std::tuple<K, Z>> group_by(std::function<K(E)>, std::function<Z(Z, E)>, Z);

            chunk<E> __data;
    };

    template <typename E>
    template <typename T>
    Collection<T> Collection<E>::map(std::function<T(E)> f) {
        chunk<T> v;
        v.reserve(__data.size());

        for (auto i : __data) {
            v.push_back(f(i));
        }

        return Collection<T>(v);
    }

    template <typename E>
    template <typename K, typename Z>
    Collection<std::tuple<K, Z>> Collection<E>::group_by(std::function<K(E)> g, std::function<Z(Z, E)> f, Z z) {
        std::map<K, Z> m;

        for (auto i: __data) {
            K k = g(i);
            if (m.find(k)) {
                m[k] = f(i, m[k]);
            } else {
                m[k] = f(z, i);
            }
        }

        chunk<std::tuple<K, Z>> v;
        v.reserve(m.count());

        for (auto i: m) {
            v.push_back(std::make_tuple(i.first, i.second));
        }

        return Collection<std::tuple<K, Z>>(v);
    }
}

#endif
<|endoftext|>