<commit_before>#include <cstdlib>
#include "config.h"
#include "slaveunit.h"
#include "common/filesystem.h"
#include "common/logger.h"

int main(int argc, const char* argv[])
{
	using Airwave::FileSystem;

	loggerInit(HOST_BASENAME);

	LOG("Starting Airwave slave unit %s", PROJECT_VERSION);

	if(argc != 3) {
		LOG("Usage: %s <VST plugin path> <control port id>", argv[0]);
		loggerFree();
		return -1;
	}

	loggerSetSenderId(FileSystem::baseName(argv[1]));

	Airwave::SlaveUnit slaveUnit;
	if(!slaveUnit.initialize(argv[1], atoi(argv[2]))) {
		LOG("Unable to initialize slave unit.");
		loggerFree();
		return -2;
	}

	LOG("Slave unit initialized, starting to process events.");

	while(slaveUnit.processRequest()) {
		MSG message;

		if(PeekMessage(&message, 0, 0, 0, PM_REMOVE)) {
			TranslateMessage(&message);
			DispatchMessage(&message);
		}
	}

	LOG("Slave unit terminated.");
	loggerFree();
	return 0;
}
<commit_msg>Improve window events handling<commit_after>#include <cstdlib>
#include "config.h"
#include "slaveunit.h"
#include "common/filesystem.h"
#include "common/logger.h"

int main(int argc, const char* argv[])
{
	using Airwave::FileSystem;

	loggerInit(HOST_BASENAME);

	LOG("Starting Airwave slave unit %s", PROJECT_VERSION);

	if(argc != 3) {
		LOG("Usage: %s <VST plugin path> <control port id>", argv[0]);
		loggerFree();
		return -1;
	}

	loggerSetSenderId(FileSystem::baseName(argv[1]));

	Airwave::SlaveUnit slaveUnit;
	if(!slaveUnit.initialize(argv[1], atoi(argv[2]))) {
		LOG("Unable to initialize slave unit.");
		loggerFree();
		return -2;
	}

	LOG("Slave unit initialized, starting to process events.");

	while(slaveUnit.processRequest()) {
		MSG message;

		while(PeekMessage(&message, 0, 0, 0, PM_REMOVE)) {
			TranslateMessage(&message);
			DispatchMessage(&message);
		}
	}

	LOG("Slave unit terminated.");
	loggerFree();
	return 0;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <string>
#include "include/Options.hpp"
#include "include/Cluster.hpp"
//#include "Driver/DissectionSolver.hpp"
int main(int argc, char *argv[]){

    std::cout << argv[0] << std::endl;
    Options options;
    int n_subdomOnCluster = 8;
    std::string path2data = "../data";
    options.set_values(n_subdomOnCluster, path2data);

    Cluster cluster(options);
    std::cout << "----------------- done -----------------\n" ;
    return 0;
}
<commit_msg>htfeti.cpp: cleaning<commit_after>#include <iostream>
#include <string>
#include "include/Options.hpp"
#include "include/Cluster.hpp"
//#include "Driver/DissectionSolver.hpp"
int main(int argc, char *argv[]){
    std::cout << argv[0] << std::endl;
    Options options;
    int n_subdomOnCluster = 8;
    std::string path2data = "../data";
    options.set_values(n_subdomOnCluster, path2data);

    Cluster cluster(options);
    std::cout << "----------------- done -----------------\n" ;
    return 0;
}
<|endoftext|>


<commit_before>//
// Created by malek on 3/20/18.
//

#include <cuda_runtime.h>
#include <cstdint>
#include <iostream>

using size_type = uint64_t;

namespace {
    inline void handle_cuda_error(cudaError_t e)
    {
        if (e != cudaError_t::cudaSuccess)
        {
            std::cerr << cudaGetErrorString(e) << std::endl;
            exit(1);
        }

    }
}

extern "C"
int tiramisu_cuda_memcpy_to_device(void * to, void * from, uint64_t size)
{
   handle_cuda_error(cudaMemcpy(to, from, size, cudaMemcpyKind::cudaMemcpyHostToDevice));
   return 0;
}

extern "C"
int tiramisu_cuda_memcpy_to_host(void * to, void * from, uint64_t size)
{
    handle_cuda_error(cudaMemcpy(to, from, size, cudaMemcpyKind::cudaMemcpyDeviceToHost));
    return 0;
}

extern "C"
void * tiramisu_cuda_malloc(uint64_t size)
{
    void * result;
    handle_cuda_error(cudaMalloc(&result, size));
    return result;
}

extern "C"
int tiramisu_cuda_free(void * ptr)
{
    handle_cuda_error(cudaFree(ptr));
    return 0;
}
<commit_msg>more helpful debugging for cuda wrappers<commit_after>//
// Created by malek on 3/20/18.
//

#include <cuda_runtime.h>
#include <cstdint>
#include <iostream>

using size_type = uint64_t;

namespace {
    inline void handle_cuda_error(cudaError_t e, const std::string & function_name)
    {
        if (e != cudaError_t::cudaSuccess)
        {
            std::cerr << "Error at " << function_name << ": " << cudaGetErrorString(e) << std::endl;
            exit(1);
        }

    }
}

extern "C"
int tiramisu_cuda_memcpy_to_device(void * to, void * from, uint64_t size)
{
   handle_cuda_error(cudaMemcpy(to, from, size, cudaMemcpyKind::cudaMemcpyHostToDevice), __FUNCTION__);
   return 0;
}

extern "C"
int tiramisu_cuda_memcpy_to_host(void * to, void * from, uint64_t size)
{
    handle_cuda_error(cudaMemcpy(to, from, size, cudaMemcpyKind::cudaMemcpyDeviceToHost), __FUNCTION__);
    return 0;
}

extern "C"
void * tiramisu_cuda_malloc(uint64_t size)
{
    void * result;
    handle_cuda_error(cudaMalloc(&result, size), __FUNCTION__);
    return result;
}

extern "C"
int tiramisu_cuda_free(void * ptr)
{
    handle_cuda_error(cudaFree(ptr), __FUNCTION__);
    return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright 2014, Andrea Del Prete, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control 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.
 * sot-torque-control 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 Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <iostream>

#include <pinocchio/fwd.hpp>

#include <boost/property_tree/ptree.hpp>

#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/core/exception-tools.hh>
#include <sot/core/parameter-server.hh>

namespace dynamicgraph {
namespace sot {
namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace std;

// Size to be aligned "-------------------------------------------------------"
#define PROFILE_PWM_DESIRED_COMPUTATION                                        \
  "Control manager                                        "
#define PROFILE_DYNAMIC_GRAPH_PERIOD                                           \
  "Control period                                         "

#define INPUT_SIGNALS
#define OUTPUT_SIGNALS

/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef ParameterServer EntityClassName;

/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ParameterServer, "ParameterServer");

/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
ParameterServer::ParameterServer(const std::string &name)
    : Entity(name), m_robot_util(RefVoidRobotUtil()), m_initSucceeded(false),
      m_emergency_stop_triggered(false), m_is_first_iter(true), m_iter(0),
      m_sleep_time(0.0) {

  //~ Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);

  /* Commands. */
  addCommand("init",
             makeCommandVoid3(*this, &ParameterServer::init,
                              docCommandVoid3("Initialize the entity.",
                                              "Time period in seconds (double)",
                                              "URDF file path (string)",
                                              "Robot reference (string)")));
  addCommand(
      "init_simple",
      makeCommandVoid1(*this, &ParameterServer::init_simple,
                       docCommandVoid1("Initialize the entity.",
                                       "Time period in seconds (double)")));

  addCommand("setNameToId",
             makeCommandVoid2(*this, &ParameterServer::setNameToId,
                              docCommandVoid2("Set map for a name to an Id",
                                              "(string) joint name",
                                              "(double) joint id")));

  addCommand(
      "setForceNameToForceId",
      makeCommandVoid2(
          *this, &ParameterServer::setForceNameToForceId,
          docCommandVoid2(
              "Set map from a force sensor name to a force sensor Id",
              "(string) force sensor name", "(double) force sensor id")));

  addCommand("setJointLimitsFromId",
             makeCommandVoid3(
                 *this, &ParameterServer::setJointLimitsFromId,
                 docCommandVoid3("Set the joint limits for a given joint ID",
                                 "(double) joint id", "(double) lower limit",
                                 "(double) uppper limit")));

  addCommand(
      "setForceLimitsFromId",
      makeCommandVoid3(
          *this, &ParameterServer::setForceLimitsFromId,
          docCommandVoid3("Set the force limits for a given force sensor ID",
                          "(double) force sensor id", "(double) lower limit",
                          "(double) uppper limit")));

  addCommand(
      "setJointsUrdfToSot",
      makeCommandVoid1(*this, &ParameterServer::setJoints,
                       docCommandVoid1("Map Joints From URDF to SoT.",
                                       "Vector of integer for mapping")));

  addCommand(
      "setRightFootSoleXYZ",
      makeCommandVoid1(*this, &ParameterServer::setRightFootSoleXYZ,
                       docCommandVoid1("Set the right foot sole 3d position.",
                                       "Vector of 3 doubles")));
  addCommand(
      "setRightFootForceSensorXYZ",
      makeCommandVoid1(*this, &ParameterServer::setRightFootForceSensorXYZ,
                       docCommandVoid1("Set the right foot sensor 3d position.",
                                       "Vector of 3 doubles")));

  addCommand("setFootFrameName",
             makeCommandVoid2(
                 *this, &ParameterServer::setFootFrameName,
                 docCommandVoid2("Set the Frame Name for the Foot Name.",
                                 "(string) Foot name", "(string) Frame name")));
  addCommand("setHandFrameName",
             makeCommandVoid2(
                 *this, &ParameterServer::setHandFrameName,
                 docCommandVoid2("Set the Frame Name for the Hand Name.",
                                 "(string) Hand name", "(string) Frame name")));
  addCommand("setImuJointName",
             makeCommandVoid1(
                 *this, &ParameterServer::setImuJointName,
                 docCommandVoid1("Set the Joint Name wich IMU is attached to.",
                                 "(string) Joint name")));
  addCommand("displayRobotUtil",
             makeCommandVoid0(
                 *this, &ParameterServer::displayRobotUtil,
                 docCommandVoid0("Display the current robot util data set.")));

  addCommand(
      "setParameterBool",
      makeCommandVoid2(
          *this, &ParameterServer::setParameter<bool>,
          docCommandVoid2("Set a parameter named ParameterName to value "
                          "ParameterValue (string format).",
                          "(string) ParameterName", "(bool) ParameterValue")));
  addCommand(
      "setParameterInt",
      makeCommandVoid2(
          *this, &ParameterServer::setParameter<int>,
          docCommandVoid2("Set a parameter named ParameterName to value "
                          "ParameterValue (string format).",
                          "(string) ParameterName", "(int) ParameterValue")));
  addCommand("setParameterDbl",
             makeCommandVoid2(
                 *this, &ParameterServer::setParameter<double>,
                 docCommandVoid2("Set a parameter named ParameterName to value "
                                 "ParameterValue (string format).",
                                 "(string) ParameterName",
                                 "(double) ParameterValue")));

  addCommand("setParameter",
             makeCommandVoid2(
                 *this, &ParameterServer::setParameter<std::string>,
                 docCommandVoid2("Set a parameter named ParameterName to value "
                                 "ParameterValue (string format).",
                                 "(string) ParameterName",
                                 "(string) ParameterValue")));

  addCommand(
      "getParameter",
      makeCommandReturnType1(*this, &ParameterServer::getParameter<std::string>,
                             docCommandReturnType1<std::string>(
                                 "Return the parameter value for parameter"
                                 " named ParameterName.",
                                 "(string) ParameterName")));

  addCommand(
      "getParameterInt",
      makeCommandReturnType1(
          *this, &ParameterServer::getParameter<int>,
          docCommandReturnType1<int>("Return the parameter value for parameter"
                                     " named ParameterName.",
                                     "(int) ParameterName")));

  addCommand(
      "getParameterDbl",
      makeCommandReturnType1(*this, &ParameterServer::getParameter<double>,
                             docCommandReturnType1<double>(
                                 "Return the parameter value for parameter"
                                 " named ParameterName.",
                                 "(double) ParameterName")));

  addCommand(
      "getParameterBool",
      makeCommandReturnType1(
          *this, &ParameterServer::getParameter<bool>,
          docCommandReturnType1<bool>("Return the parameter value for parameter"
                                      " named ParameterName.",
                                      "(string) ParameterName")));
}

void ParameterServer::init_simple(const double &dt) {

  if (dt <= 0.0)
    return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);

  m_dt = dt;

  m_emergency_stop_triggered = false;
  m_initSucceeded = true;

  std::string localName("robot");
  std::shared_ptr<std::vector<std::string> > listOfRobots =
      sot::getListOfRobots();

  if (listOfRobots->size() == 1)
    localName = (*listOfRobots)[0];
  else {
    std::ostringstream oss;
    oss << "No robot registered in the parameter server list";
    oss << " listOfRobots->size: " << listOfRobots->size();
    throw ExceptionTools(ExceptionTools::ErrorCodeEnum::PARAMETER_SERVER,
                         oss.str());
  }

  if (!isNameInRobotUtil(localName)) {
    m_robot_util = createRobotUtil(localName);
  } else {
    m_robot_util = getRobotUtil(localName);
  }

  addCommand(
      "getJointsUrdfToSot",
      makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
                       docDirectSetter("Display map Joints From URDF to SoT.",
                                       "Vector of integer for mapping")));
}

void ParameterServer::init(const double &dt, const std::string &urdfFile,
                           const std::string &robotRef) {
  if (dt <= 0.0)
    return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
  m_dt = dt;
  m_emergency_stop_triggered = false;
  m_initSucceeded = true;

  std::string localName(robotRef);
  if (!isNameInRobotUtil(localName)) {
    m_robot_util = createRobotUtil(localName);
  } else {
    m_robot_util = getRobotUtil(localName);
  }

  m_robot_util->m_urdf_filename = urdfFile;
}

/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */

/* --- COMMANDS ---------------------------------------------------------- */

void ParameterServer::setNameToId(const std::string &jointName,
                                  const double &jointId) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot set joint name from joint id  before initialization!");
    return;
  }
  m_robot_util->set_name_to_id(jointName, static_cast<Index>(jointId));
}

void ParameterServer::setJointLimitsFromId(const double &jointId,
                                           const double &lq, const double &uq) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot set joints limits from joint id  before initialization!");
    return;
  }

  m_robot_util->set_joint_limits_for_id((Index)jointId, lq, uq);
}

void ParameterServer::setForceLimitsFromId(const double &jointId,
                                           const dynamicgraph::Vector &lq,
                                           const dynamicgraph::Vector &uq) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot set force limits from force id  before initialization!");
    return;
  }

  m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId, lq, uq);
}

void ParameterServer::setForceNameToForceId(const std::string &forceName,
                                            const double &forceId) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot set force sensor name from force sensor id "
                            " before initialization!");
    return;
  }

  m_robot_util->m_force_util.set_name_to_force_id(forceName,
                                                  static_cast<Index>(forceId));
}

void ParameterServer::setJoints(const dynamicgraph::Vector &urdf_to_sot) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!");
    return;
  }
  m_robot_util->set_urdf_to_sot(urdf_to_sot);
}

void ParameterServer::setRightFootSoleXYZ(const dynamicgraph::Vector &xyz) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot set right foot sole XYZ before initialization!");
    return;
  }

  m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
}

void ParameterServer::setRightFootForceSensorXYZ(
    const dynamicgraph::Vector &xyz) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot set right foot force sensor XYZ before initialization!");
    return;
  }

  m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz;
}

void ParameterServer::setFootFrameName(const std::string &FootName,
                                       const std::string &FrameName) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot set foot frame name!");
    return;
  }
  if (FootName == "Left")
    m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
  else if (FootName == "Right")
    m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
  else
    SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
}

void ParameterServer::setHandFrameName(const std::string& HandName, const std::string& FrameName) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot set hand frame name!");
    return;
  }
  if (HandName == "Left")
    m_robot_util->m_hand_util.m_Left_Hand_Frame_Name = FrameName;
  else if (HandName == "Right")
    m_robot_util->m_hand_util.m_Right_Hand_Frame_Name = FrameName;
  else
    SEND_WARNING_STREAM_MSG("Did not understand the hand name !" + HandName);
}

void ParameterServer::setImuJointName(const std::string &JointName) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot set IMU joint name!");
    return;
  }
  m_robot_util->m_imu_joint_name = JointName;
}

void ParameterServer::displayRobotUtil() { m_robot_util->display(std::cout); }

/* --- PROTECTED MEMBER METHODS
 * ---------------------------------------------------------- */

bool ParameterServer::convertJointNameToJointId(const std::string &name,
                                                unsigned int &id) {
  // Check if the joint name exists
  sot::Index jid = m_robot_util->get_id_from_name(name);
  if (jid < 0) {
    SEND_MSG("The specified joint name does not exist: " + name,
             MSG_TYPE_ERROR);
    std::stringstream ss;
    for (long unsigned int it = 0; it < m_robot_util->m_nbJoints; it++)
      ss << m_robot_util->get_name_from_id(it) << ", ";
    SEND_MSG("Possible joint names are: " + ss.str(), MSG_TYPE_INFO);
    return false;
  }
  id = (unsigned int)jid;
  return true;
}

bool ParameterServer::isJointInRange(unsigned int id, double q) {
  const JointLimits &JL = m_robot_util->get_joint_limits_from_id((Index)id);

  double jl = JL.lower;
  if (q < jl) {
    SEND_MSG("Desired joint angle " + toString(q) +
                 " is smaller than lower limit: " + toString(jl),
             MSG_TYPE_ERROR);
    return false;
  }
  double ju = JL.upper;
  if (q > ju) {
    SEND_MSG("Desired joint angle " + toString(q) +
                 " is larger than upper limit: " + toString(ju),
             MSG_TYPE_ERROR);
    return false;
  }
  return true;
}

/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */

void ParameterServer::display(std::ostream &os) const {
  os << "ParameterServer " << getName();
}
} // namespace sot
} // namespace dynamicgraph
<commit_msg>Update src/tools/parameter-server.cpp<commit_after>/*
 * Copyright 2014, Andrea Del Prete, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control 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.
 * sot-torque-control 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 Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <iostream>

#include <pinocchio/fwd.hpp>

#include <boost/property_tree/ptree.hpp>

#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/core/exception-tools.hh>
#include <sot/core/parameter-server.hh>

namespace dynamicgraph {
namespace sot {
namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace std;

// Size to be aligned "-------------------------------------------------------"
#define PROFILE_PWM_DESIRED_COMPUTATION                                        \
  "Control manager                                        "
#define PROFILE_DYNAMIC_GRAPH_PERIOD                                           \
  "Control period                                         "

#define INPUT_SIGNALS
#define OUTPUT_SIGNALS

/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef ParameterServer EntityClassName;

/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ParameterServer, "ParameterServer");

/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
ParameterServer::ParameterServer(const std::string &name)
    : Entity(name), m_robot_util(RefVoidRobotUtil()), m_initSucceeded(false),
      m_emergency_stop_triggered(false), m_is_first_iter(true), m_iter(0),
      m_sleep_time(0.0) {

  //~ Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);

  /* Commands. */
  addCommand("init",
             makeCommandVoid3(*this, &ParameterServer::init,
                              docCommandVoid3("Initialize the entity.",
                                              "Time period in seconds (double)",
                                              "URDF file path (string)",
                                              "Robot reference (string)")));
  addCommand(
      "init_simple",
      makeCommandVoid1(*this, &ParameterServer::init_simple,
                       docCommandVoid1("Initialize the entity.",
                                       "Time period in seconds (double)")));

  addCommand("setNameToId",
             makeCommandVoid2(*this, &ParameterServer::setNameToId,
                              docCommandVoid2("Set map for a name to an Id",
                                              "(string) joint name",
                                              "(double) joint id")));

  addCommand(
      "setForceNameToForceId",
      makeCommandVoid2(
          *this, &ParameterServer::setForceNameToForceId,
          docCommandVoid2(
              "Set map from a force sensor name to a force sensor Id",
              "(string) force sensor name", "(double) force sensor id")));

  addCommand("setJointLimitsFromId",
             makeCommandVoid3(
                 *this, &ParameterServer::setJointLimitsFromId,
                 docCommandVoid3("Set the joint limits for a given joint ID",
                                 "(double) joint id", "(double) lower limit",
                                 "(double) uppper limit")));

  addCommand(
      "setForceLimitsFromId",
      makeCommandVoid3(
          *this, &ParameterServer::setForceLimitsFromId,
          docCommandVoid3("Set the force limits for a given force sensor ID",
                          "(double) force sensor id", "(double) lower limit",
                          "(double) uppper limit")));

  addCommand(
      "setJointsUrdfToSot",
      makeCommandVoid1(*this, &ParameterServer::setJoints,
                       docCommandVoid1("Map Joints From URDF to SoT.",
                                       "Vector of integer for mapping")));

  addCommand(
      "setRightFootSoleXYZ",
      makeCommandVoid1(*this, &ParameterServer::setRightFootSoleXYZ,
                       docCommandVoid1("Set the right foot sole 3d position.",
                                       "Vector of 3 doubles")));
  addCommand(
      "setRightFootForceSensorXYZ",
      makeCommandVoid1(*this, &ParameterServer::setRightFootForceSensorXYZ,
                       docCommandVoid1("Set the right foot sensor 3d position.",
                                       "Vector of 3 doubles")));

  addCommand("setFootFrameName",
             makeCommandVoid2(
                 *this, &ParameterServer::setFootFrameName,
                 docCommandVoid2("Set the Frame Name for the Foot Name.",
                                 "(string) Foot name", "(string) Frame name")));
  addCommand("setHandFrameName",
             makeCommandVoid2(
                 *this, &ParameterServer::setHandFrameName,
                 docCommandVoid2("Set the Frame Name for the Hand Name.",
                                 "(string) Hand name", "(string) Frame name")));
  addCommand("setImuJointName",
             makeCommandVoid1(
                 *this, &ParameterServer::setImuJointName,
                 docCommandVoid1("Set the Joint Name wich IMU is attached to.",
                                 "(string) Joint name")));
  addCommand("displayRobotUtil",
             makeCommandVoid0(
                 *this, &ParameterServer::displayRobotUtil,
                 docCommandVoid0("Display the current robot util data set.")));

  addCommand(
      "setParameterBool",
      makeCommandVoid2(
          *this, &ParameterServer::setParameter<bool>,
          docCommandVoid2("Set a parameter named ParameterName to value "
                          "ParameterValue (string format).",
                          "(string) ParameterName", "(bool) ParameterValue")));
  addCommand(
      "setParameterInt",
      makeCommandVoid2(
          *this, &ParameterServer::setParameter<int>,
          docCommandVoid2("Set a parameter named ParameterName to value "
                          "ParameterValue (string format).",
                          "(string) ParameterName", "(int) ParameterValue")));
  addCommand("setParameterDbl",
             makeCommandVoid2(
                 *this, &ParameterServer::setParameter<double>,
                 docCommandVoid2("Set a parameter named ParameterName to value "
                                 "ParameterValue (string format).",
                                 "(string) ParameterName",
                                 "(double) ParameterValue")));

  addCommand("setParameter",
             makeCommandVoid2(
                 *this, &ParameterServer::setParameter<std::string>,
                 docCommandVoid2("Set a parameter named ParameterName to value "
                                 "ParameterValue (string format).",
                                 "(string) ParameterName",
                                 "(string) ParameterValue")));

  addCommand(
      "getParameter",
      makeCommandReturnType1(*this, &ParameterServer::getParameter<std::string>,
                             docCommandReturnType1<std::string>(
                                 "Return the parameter value for parameter"
                                 " named ParameterName.",
                                 "(string) ParameterName")));

  addCommand(
      "getParameterInt",
      makeCommandReturnType1(
          *this, &ParameterServer::getParameter<int>,
          docCommandReturnType1<int>("Return the parameter value for parameter"
                                     " named ParameterName.",
                                     "(int) ParameterName")));

  addCommand(
      "getParameterDbl",
      makeCommandReturnType1(*this, &ParameterServer::getParameter<double>,
                             docCommandReturnType1<double>(
                                 "Return the parameter value for parameter"
                                 " named ParameterName.",
                                 "(double) ParameterName")));

  addCommand(
      "getParameterBool",
      makeCommandReturnType1(
          *this, &ParameterServer::getParameter<bool>,
          docCommandReturnType1<bool>("Return the parameter value for parameter"
                                      " named ParameterName.",
                                      "(string) ParameterName")));
}

void ParameterServer::init_simple(const double &dt) {

  if (dt <= 0.0)
    return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);

  m_dt = dt;

  m_emergency_stop_triggered = false;
  m_initSucceeded = true;

  std::string localName("robot");
  std::shared_ptr<std::vector<std::string> > listOfRobots =
      sot::getListOfRobots();

  if (listOfRobots->size() == 1)
    localName = (*listOfRobots)[0];
  else {
    std::ostringstream oss;
    oss << "No robot registered in the parameter server list";
    oss << " listOfRobots->size: " << listOfRobots->size();
    throw ExceptionTools(ExceptionTools::ErrorCodeEnum::PARAMETER_SERVER,
                         oss.str());
  }

  if (!isNameInRobotUtil(localName)) {
    m_robot_util = createRobotUtil(localName);
  } else {
    m_robot_util = getRobotUtil(localName);
  }

  addCommand(
      "getJointsUrdfToSot",
      makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
                       docDirectSetter("Display map Joints From URDF to SoT.",
                                       "Vector of integer for mapping")));
}

void ParameterServer::init(const double &dt, const std::string &urdfFile,
                           const std::string &robotRef) {
  if (dt <= 0.0)
    return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
  m_dt = dt;
  m_emergency_stop_triggered = false;
  m_initSucceeded = true;

  std::string localName(robotRef);
  if (!isNameInRobotUtil(localName)) {
    m_robot_util = createRobotUtil(localName);
  } else {
    m_robot_util = getRobotUtil(localName);
  }

  m_robot_util->m_urdf_filename = urdfFile;
}

/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */

/* --- COMMANDS ---------------------------------------------------------- */

void ParameterServer::setNameToId(const std::string &jointName,
                                  const double &jointId) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot set joint name from joint id  before initialization!");
    return;
  }
  m_robot_util->set_name_to_id(jointName, static_cast<Index>(jointId));
}

void ParameterServer::setJointLimitsFromId(const double &jointId,
                                           const double &lq, const double &uq) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot set joints limits from joint id  before initialization!");
    return;
  }

  m_robot_util->set_joint_limits_for_id((Index)jointId, lq, uq);
}

void ParameterServer::setForceLimitsFromId(const double &jointId,
                                           const dynamicgraph::Vector &lq,
                                           const dynamicgraph::Vector &uq) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot set force limits from force id  before initialization!");
    return;
  }

  m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId, lq, uq);
}

void ParameterServer::setForceNameToForceId(const std::string &forceName,
                                            const double &forceId) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot set force sensor name from force sensor id "
                            " before initialization!");
    return;
  }

  m_robot_util->m_force_util.set_name_to_force_id(forceName,
                                                  static_cast<Index>(forceId));
}

void ParameterServer::setJoints(const dynamicgraph::Vector &urdf_to_sot) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!");
    return;
  }
  m_robot_util->set_urdf_to_sot(urdf_to_sot);
}

void ParameterServer::setRightFootSoleXYZ(const dynamicgraph::Vector &xyz) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot set right foot sole XYZ before initialization!");
    return;
  }

  m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
}

void ParameterServer::setRightFootForceSensorXYZ(
    const dynamicgraph::Vector &xyz) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG(
        "Cannot set right foot force sensor XYZ before initialization!");
    return;
  }

  m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz;
}

void ParameterServer::setFootFrameName(const std::string &FootName,
                                       const std::string &FrameName) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot set foot frame name!");
    return;
  }
  if (FootName == "Left")
    m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
  else if (FootName == "Right")
    m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
  else
    SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
}

void ParameterServer::setHandFrameName(const std::string& HandName, const std::string& FrameName) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot set hand frame name!");
    return;
  }
  if (HandName == "Left")
    m_robot_util->m_hand_util.m_Left_Hand_Frame_Name = FrameName;
  else if (HandName == "Right")
    m_robot_util->m_hand_util.m_Right_Hand_Frame_Name = FrameName;
  else
    SEND_WARNING_STREAM_MSG("Available hand names are 'Left' and 'Right', not '" + HandName + "' !");
}

void ParameterServer::setImuJointName(const std::string &JointName) {
  if (!m_initSucceeded) {
    SEND_WARNING_STREAM_MSG("Cannot set IMU joint name!");
    return;
  }
  m_robot_util->m_imu_joint_name = JointName;
}

void ParameterServer::displayRobotUtil() { m_robot_util->display(std::cout); }

/* --- PROTECTED MEMBER METHODS
 * ---------------------------------------------------------- */

bool ParameterServer::convertJointNameToJointId(const std::string &name,
                                                unsigned int &id) {
  // Check if the joint name exists
  sot::Index jid = m_robot_util->get_id_from_name(name);
  if (jid < 0) {
    SEND_MSG("The specified joint name does not exist: " + name,
             MSG_TYPE_ERROR);
    std::stringstream ss;
    for (long unsigned int it = 0; it < m_robot_util->m_nbJoints; it++)
      ss << m_robot_util->get_name_from_id(it) << ", ";
    SEND_MSG("Possible joint names are: " + ss.str(), MSG_TYPE_INFO);
    return false;
  }
  id = (unsigned int)jid;
  return true;
}

bool ParameterServer::isJointInRange(unsigned int id, double q) {
  const JointLimits &JL = m_robot_util->get_joint_limits_from_id((Index)id);

  double jl = JL.lower;
  if (q < jl) {
    SEND_MSG("Desired joint angle " + toString(q) +
                 " is smaller than lower limit: " + toString(jl),
             MSG_TYPE_ERROR);
    return false;
  }
  double ju = JL.upper;
  if (q > ju) {
    SEND_MSG("Desired joint angle " + toString(q) +
                 " is larger than upper limit: " + toString(ju),
             MSG_TYPE_ERROR);
    return false;
  }
  return true;
}

/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */

void ParameterServer::display(std::ostream &os) const {
  os << "ParameterServer " << getName();
}
} // namespace sot
} // namespace dynamicgraph
<|endoftext|>


<commit_before>#include <algorithm>
#include <vector>

#include "unittest/gtest.hpp"

#include "btree/node.hpp"
#include "btree/leaf_node.hpp"

namespace unittest {

// TODO: This is rather duplicative of fsck::check_value.
void expect_valid_value_shallowly(const btree_value *value) {
    EXPECT_TRUE(!(value->metadata_flags & ~(MEMCACHED_FLAGS | MEMCACHED_CAS | MEMCACHED_EXPTIME | LARGE_VALUE)));

    size_t size = value->value_size();

    if (value->is_large()) {
        EXPECT_GT(size, MAX_IN_NODE_VALUE_SIZE);

        // This isn't fsck, we can't follow large buf pointers.
    } else {
        EXPECT_LE(size, MAX_IN_NODE_VALUE_SIZE);
    }
}

// TODO: This is rather duplicative of fsck::check_subtree_leaf_node.
void verify(block_size_t block_size, const leaf_node_t *buf) {

    ASSERT_LE(offsetof(leaf_node_t, pair_offsets) + buf->npairs * 2, buf->frontmost_offset);
    ASSERT_LE(buf->frontmost_offset, block_size.value());

    std::vector<uint16_t> offsets(buf->pair_offsets, buf->pair_offsets + buf->npairs);
    std::sort(offsets.begin(), offsets.end());

    uint16_t expected = buf->frontmost_offset;
    for (std::vector<uint16_t>::const_iterator p = offsets.begin(), e = offsets.end(); p < e; ++p) {
        ASSERT_LE(expected, block_size.value());
        ASSERT_EQ(expected, *p);
        expected += leaf_node_handler::pair_size(leaf_node_handler::get_pair(buf, *p));
    }
    ASSERT_EQ(block_size.value(), expected);

    const btree_key *last_key = NULL;
    for (const uint16_t *p = buf->pair_offsets, *e = p + buf->npairs; p < e; ++p) {
        const btree_leaf_pair *pair = leaf_node_handler::get_pair(buf, *p);
        if (last_key != NULL) {
            const btree_key *next_key = &pair->key;
            EXPECT_LT(leaf_key_comp::compare(last_key, next_key), 0);
            last_key = next_key;
        }
        expect_valid_value_shallowly(pair->value());
    }
}

TEST(LeafNodeTest, Offsets) {
    // Check leaf_node_t.
    EXPECT_EQ(0, offsetof(leaf_node_t, magic));
    EXPECT_EQ(4, offsetof(leaf_node_t, times));
    EXPECT_EQ(12, offsetof(leaf_node_t, npairs));
    EXPECT_EQ(14, offsetof(leaf_node_t, frontmost_offset));
    EXPECT_EQ(16, offsetof(leaf_node_t, pair_offsets));
    EXPECT_EQ(16, sizeof(leaf_node_t));

    // Check leaf_timestamps_t, since that's specifically part of leaf_node_t.
    EXPECT_EQ(0, offsetof(leaf_timestamps_t, last_modified));
    EXPECT_EQ(4, offsetof(leaf_timestamps_t, earlier));

    // Changing NUM_LEAF_NODE_EARLIER_TIMES changes the disk format.
    EXPECT_EQ(2, NUM_LEAF_NODE_EARLIER_TIMES);

    EXPECT_EQ(8, sizeof(leaf_timestamps_t));

    // Check btree_leaf_pair.
    EXPECT_EQ(0, offsetof(btree_leaf_pair, key));

    btree_leaf_pair p;
    p.key.size = 173;
    EXPECT_EQ(174, reinterpret_cast<byte *>(p.value()) - reinterpret_cast<byte *>(&p));
}

class Value {
public:
    Value(const std::string& data = "", mcflags_t mcflags = 0, cas_t cas = 0, bool has_cas = false, exptime_t exptime = 0)
        : mcflags_(mcflags), cas_(cas), exptime_(exptime),
          has_cas_(has_cas)
          data_(data) { }

    void WriteBtreeValue(btree_value *out) const {
        out->size = 0;
        ASSERT_LE(data_.size(), MAX_IN_NODE_VALUE_SIZE);
        out->value_size(data_.size());
        out->set_mcflags(mcflags_);
        out->set_exptime(exptime_);
        if (has_cas_) {
            out->set_cas(cas_);
        }
    }

    static Value Make(const btree_value *v) {
        ASSERT_FALSE(v->is_large());
        return Value(std::string(v->value(), v->value() + v->value_size()),
                     v->mcflags(), v->has_cas() ? v->cas() : 0, v->has_cas(), v->exptime());
    }

    bool operator==(const Value& rhs) {
        return mcflags_ == rhs.mcflags_
            && exptime_ == rhs.exptime_
            && data_ == rhs.data_
            && has_cas_ == rhs.has_cas_
            && (has_cas_ ? cas_ == rhs.cas_ : true);
    }

private:
    mcflags_t mcflags_;
    cas_t cas_;
    exptime_t exptime_;
    bool has_cas_;
    std::string data_;
};

template <int block_size>
class LeafNodeGrinder {
    typedef std::map<std::string, Value> expected_t;
    expected_t expected;
    leaf_node_t *node;

    void insert(const std::string& k, const Value& v) {
        std::pair<expected_t::iterator, bool> res = expected.insert(k, v);
        if (!res.second) {
            res.first->second = v;
        }

    }

};

block_size_t make_bs() {
    // TODO: Test weird block sizes.
    return block_size_t::unsafe_make(4096);
}

leaf_node_t *malloc_leaf_node(block_size_t bs) {
    return reinterpret_cast<leaf_node_t *>(malloc(bs.value()));
}

btree_key *malloc_key(const char *s) {
    size_t origlen = strlen(s);
    EXPECT_LE(origlen, MAX_KEY_SIZE);

    size_t len = std::min<size_t>(origlen, MAX_KEY_SIZE);
    btree_key *k = reinterpret_cast<btree_key *>(malloc(sizeof(btree_key) + len));
    k->size = len;
    memcpy(k->contents, s, len);
    return k;
}

btree_value *malloc_value(const char *s) {
    size_t origlen = strlen(s);
    EXPECT_LE(origlen, MAX_IN_NODE_VALUE_SIZE);

    size_t len = std::min<size_t>(origlen, MAX_IN_NODE_VALUE_SIZE);

    btree_value *v = reinterpret_cast<btree_value *>(malloc(sizeof(btree_value) + len));
    v->size = len;
    v->metadata_flags = 0;
    memcpy(v->value(), s, len);
    return v;
}

TEST(LeafNodeTest, Initialization) {
    block_size_t bs = make_bs();
    leaf_node_t *node = malloc_leaf_node(bs);

    leaf_node_handler::init(bs, node, repli_timestamp::invalid);
    verify(bs, node);
    free(node);
}

// Runs an InsertLookupRemove test with key and value.
void InsertLookupRemoveHelper(const char *key, const char *value) {
    block_size_t bs = make_bs();
    leaf_node_t *node = malloc_leaf_node(bs);

    leaf_node_handler::init(bs, node, repli_timestamp::invalid);
    verify(bs, node);

    btree_key *k = malloc_key(key);
    btree_value *v = malloc_value(value);

    ASSERT_TRUE(leaf_node_handler::insert(bs, node, k, v, repli_timestamp::invalid));
    verify(bs, node);
    ASSERT_EQ(1, node->npairs);

    union {
        byte readval_padding[MAX_TOTAL_NODE_CONTENTS_SIZE + sizeof(btree_value)];
        btree_value readval;
    };

    (void)readval_padding;

    ASSERT_TRUE(leaf_node_handler::lookup(node, k, &readval));

    ASSERT_EQ(0, memcmp(v, &readval, v->full_size()));

    leaf_node_handler::remove(bs, node, k);
    verify(bs, node);
    ASSERT_EQ(0, node->npairs);

    free(k);
    free(v);
    free(node);
}

TEST(LeafNodeTest, ElementaryInsertLookupRemove) {
    InsertLookupRemoveHelper("the_key", "the_value");
}
TEST(LeafNodeTest, EmptyValueInsertLookupRemove) {
    InsertLookupRemoveHelper("the_key", "");
}
TEST(LeafNodeTest, EmptyKeyInsertLookupRemove) {
    InsertLookupRemoveHelper("", "the_value");
}
TEST(LeafNodeTest, EmptyKeyValueInsertLookupRemove) {
    InsertLookupRemoveHelper("", "");
}

}  // namespace unittest

<commit_msg>Some more changes to leaf_node_test.cc<commit_after>#include <algorithm>
#include <vector>

#include "unittest/gtest.hpp"

#include "btree/node.hpp"
#include "btree/leaf_node.hpp"

namespace unittest {

// TODO: This is rather duplicative of fsck::check_value.
void expect_valid_value_shallowly(const btree_value *value) {
    EXPECT_TRUE(!(value->metadata_flags & ~(MEMCACHED_FLAGS | MEMCACHED_CAS | MEMCACHED_EXPTIME | LARGE_VALUE)));

    size_t size = value->value_size();

    if (value->is_large()) {
        EXPECT_GT(size, MAX_IN_NODE_VALUE_SIZE);

        // This isn't fsck, we can't follow large buf pointers.
    } else {
        EXPECT_LE(size, MAX_IN_NODE_VALUE_SIZE);
    }
}

// TODO: This is rather duplicative of fsck::check_subtree_leaf_node.
void verify(block_size_t block_size, const leaf_node_t *buf) {

    ASSERT_LE(offsetof(leaf_node_t, pair_offsets) + buf->npairs * 2, buf->frontmost_offset);
    ASSERT_LE(buf->frontmost_offset, block_size.value());

    std::vector<uint16_t> offsets(buf->pair_offsets, buf->pair_offsets + buf->npairs);
    std::sort(offsets.begin(), offsets.end());

    uint16_t expected = buf->frontmost_offset;
    for (std::vector<uint16_t>::const_iterator p = offsets.begin(), e = offsets.end(); p < e; ++p) {
        ASSERT_LE(expected, block_size.value());
        ASSERT_EQ(expected, *p);
        expected += leaf_node_handler::pair_size(leaf_node_handler::get_pair(buf, *p));
    }
    ASSERT_EQ(block_size.value(), expected);

    const btree_key *last_key = NULL;
    for (const uint16_t *p = buf->pair_offsets, *e = p + buf->npairs; p < e; ++p) {
        const btree_leaf_pair *pair = leaf_node_handler::get_pair(buf, *p);
        if (last_key != NULL) {
            const btree_key *next_key = &pair->key;
            EXPECT_LT(leaf_key_comp::compare(last_key, next_key), 0);
            last_key = next_key;
        }
        expect_valid_value_shallowly(pair->value());
    }
}

TEST(LeafNodeTest, Offsets) {
    // Check leaf_node_t.
    EXPECT_EQ(0, offsetof(leaf_node_t, magic));
    EXPECT_EQ(4, offsetof(leaf_node_t, times));
    EXPECT_EQ(12, offsetof(leaf_node_t, npairs));
    EXPECT_EQ(14, offsetof(leaf_node_t, frontmost_offset));
    EXPECT_EQ(16, offsetof(leaf_node_t, pair_offsets));
    EXPECT_EQ(16, sizeof(leaf_node_t));

    // Check leaf_timestamps_t, since that's specifically part of leaf_node_t.
    EXPECT_EQ(0, offsetof(leaf_timestamps_t, last_modified));
    EXPECT_EQ(4, offsetof(leaf_timestamps_t, earlier));

    // Changing NUM_LEAF_NODE_EARLIER_TIMES changes the disk format.
    EXPECT_EQ(2, NUM_LEAF_NODE_EARLIER_TIMES);

    EXPECT_EQ(8, sizeof(leaf_timestamps_t));

    // Check btree_leaf_pair.
    EXPECT_EQ(0, offsetof(btree_leaf_pair, key));

    btree_leaf_pair p;
    p.key.size = 173;
    EXPECT_EQ(174, reinterpret_cast<byte *>(p.value()) - reinterpret_cast<byte *>(&p));
}

class Value {
public:
    Value(const std::string& data = "", mcflags_t mcflags = 0, cas_t cas = 0, bool has_cas = false, exptime_t exptime = 0)
        : mcflags_(mcflags), cas_(cas), exptime_(exptime),
          has_cas_(has_cas)
          data_(data) {
        ASSERT_LE(data_.size(), MAX_IN_NODE_VALUE_SIZE);
    }

    void WriteBtreeValue(btree_value *out) const {
        out->size = 0;
        ASSERT_LE(data_.size(), MAX_IN_NODE_VALUE_SIZE);
        out->value_size(data_.size());
        out->set_mcflags(mcflags_);
        out->set_exptime(exptime_);
        if (has_cas_) {
            out->set_cas(cas_);
        }
    }

    static Value Make(const btree_value *v) {
        ASSERT_FALSE(v->is_large());
        return Value(std::string(v->value(), v->value() + v->value_size()),
                     v->mcflags(), v->has_cas() ? v->cas() : 0, v->has_cas(), v->exptime());
    }

    bool operator==(const Value& rhs) {
        return mcflags_ == rhs.mcflags_
            && exptime_ == rhs.exptime_
            && data_ == rhs.data_
            && has_cas_ == rhs.has_cas_
            && (has_cas_ ? cas_ == rhs.cas_ : true);
    }

    int full_size() const {
        return sizeof(btree_value) + (mcflags_ ? sizeof(mcflags_t) : 0) + (has_cas_ ? sizeof(cas_t) : 0)
            + (exptime_ ? sizeof(exptime_t) : 0) + data_.size();
    }

private:
    mcflags_t mcflags_;
    cas_t cas_;
    exptime_t exptime_;
    bool has_cas_;
    std::string data_;
};

class StackKey {
public:
    explicit StackKey(const std::string& key) {
        ASSERT_LE(key.size(), MAX_KEY_SIZE);
        keyval.size = key.size();
        memcpy(keyval.contents, key.c_str(), key.size());
    }

    const btree_key *look() const {
        return &keyval;
    }

private:
    union {
        byte keyval_padding[sizeof(btree_key) + MAX_KEY_SIZE];
        btree_key keyval;
    };
};

class StackValue {
public:
    explicit StackValue(const Value& value) {
        value.WriteBtreeValue(&val);
    }
    const btree_value *look() const {
        return &val;
    }
private:
    union {
        byte val_padding[sizeof(btree_value) + MAX_TOTAL_NODE_CONTENTS_SIZE];
        btree_value val;
    };
};

template <int block_size>
class LeafNodeGrinder {
    block_size_t bs;
    typedef std::map<std::string, Value> expected_t;
    expected_t expected;
    int expected_free_space;
    leaf_node_t *node;

    void insert(const std::string& k, const Value& v) {
        if (expected_free_space < sizeof(*node->pair_offsets) + v.full_size()) {
            StackKey skey(k);
            StackValue sval(v);
            ASSERT_FALSE(leaf_node_handler::insert(bs, node, skey.look(), sval.look(), repli_timestamp::invalid));
            verify(bs, node, expected_free_space);
        }

        std::pair<expected_t::iterator, bool> res = expected.insert(k, v);
        if (res.second) {
            // The key didn't previously exist.
            expected_free_space -= v.full_size() + sizeof(*node->pair_offsets);
        } else {
            // The key previously existed.
            expected_free_space += res.first->second.full_size();
            expected_free_space -= v.full_size();
            res.first->second = v;
        }

        verify(bs, node, expected_free_space);
    }

    void remove(const std::string& k) {
        expected_t::iterator p = expected.find(k);
        if (p != expected.end()) {

        }

    }

};

block_size_t make_bs() {
    // TODO: Test weird block sizes.
    return block_size_t::unsafe_make(4096);
}

leaf_node_t *malloc_leaf_node(block_size_t bs) {
    return reinterpret_cast<leaf_node_t *>(malloc(bs.value()));
}

btree_key *malloc_key(const char *s) {
    size_t origlen = strlen(s);
    EXPECT_LE(origlen, MAX_KEY_SIZE);

    size_t len = std::min<size_t>(origlen, MAX_KEY_SIZE);
    btree_key *k = reinterpret_cast<btree_key *>(malloc(sizeof(btree_key) + len));
    k->size = len;
    memcpy(k->contents, s, len);
    return k;
}

btree_value *malloc_value(const char *s) {
    size_t origlen = strlen(s);
    EXPECT_LE(origlen, MAX_IN_NODE_VALUE_SIZE);

    size_t len = std::min<size_t>(origlen, MAX_IN_NODE_VALUE_SIZE);

    btree_value *v = reinterpret_cast<btree_value *>(malloc(sizeof(btree_value) + len));
    v->size = len;
    v->metadata_flags = 0;
    memcpy(v->value(), s, len);
    return v;
}

TEST(LeafNodeTest, Initialization) {
    block_size_t bs = make_bs();
    leaf_node_t *node = malloc_leaf_node(bs);

    leaf_node_handler::init(bs, node, repli_timestamp::invalid);
    verify(bs, node);
    free(node);
}

// Runs an InsertLookupRemove test with key and value.
void InsertLookupRemoveHelper(const char *key, const char *value) {
    block_size_t bs = make_bs();
    leaf_node_t *node = malloc_leaf_node(bs);

    leaf_node_handler::init(bs, node, repli_timestamp::invalid);
    verify(bs, node);

    btree_key *k = malloc_key(key);
    btree_value *v = malloc_value(value);

    ASSERT_TRUE(leaf_node_handler::insert(bs, node, k, v, repli_timestamp::invalid));
    verify(bs, node);
    ASSERT_EQ(1, node->npairs);

    union {
        byte readval_padding[MAX_TOTAL_NODE_CONTENTS_SIZE + sizeof(btree_value)];
        btree_value readval;
    };

    (void)readval_padding;

    ASSERT_TRUE(leaf_node_handler::lookup(node, k, &readval));

    ASSERT_EQ(0, memcmp(v, &readval, v->full_size()));

    leaf_node_handler::remove(bs, node, k);
    verify(bs, node);
    ASSERT_EQ(0, node->npairs);

    free(k);
    free(v);
    free(node);
}

TEST(LeafNodeTest, ElementaryInsertLookupRemove) {
    InsertLookupRemoveHelper("the_key", "the_value");
}
TEST(LeafNodeTest, EmptyValueInsertLookupRemove) {
    InsertLookupRemoveHelper("the_key", "");
}
TEST(LeafNodeTest, EmptyKeyInsertLookupRemove) {
    InsertLookupRemoveHelper("", "the_value");
}
TEST(LeafNodeTest, EmptyKeyValueInsertLookupRemove) {
    InsertLookupRemoveHelper("", "");
}

}  // namespace unittest

<|endoftext|>


<commit_before>/*
 * Copyright (C) 2003-2014 Max Kellermann <max@duempel.org>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * - Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 *
 * - Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the
 * distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE
 * FOUNDATION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef FOREIGN_FIFO_BUFFER_HXX
#define FOREIGN_FIFO_BUFFER_HXX

#include "WritableBuffer.hxx"

#include <utility>
#include <algorithm>

#include <cstddef>

#include <assert.h>

/**
 * A first-in-first-out buffer: you can append data at the end, and
 * read data from the beginning.  This class automatically shifts the
 * buffer as needed.  It is not thread safe.
 *
 * This class does not manage buffer memory.  It will not allocate or
 * free any memory, it only manages the contents of an existing buffer
 * given to the constructor.
 */
template<typename T>
class ForeignFifoBuffer {
public:
	typedef size_t size_type;
	typedef WritableBuffer<T> Range;
	typedef typename Range::pointer_type pointer_type;
	typedef typename Range::const_pointer_type const_pointer_type;

protected:
	size_type head, tail, capacity;
	T *data;

public:
	explicit constexpr ForeignFifoBuffer(std::nullptr_t n)
		:head(0), tail(0), capacity(0), data(n) {}

	constexpr ForeignFifoBuffer(T *_data, size_type _capacity)
		:head(0), tail(0), capacity(_capacity), data(_data) {}

	ForeignFifoBuffer(ForeignFifoBuffer &&src)
		:head(src.head), tail(src.tail),
		 capacity(src.capacity), data(src.data) {
		src.SetNull();
	}

	ForeignFifoBuffer &operator=(ForeignFifoBuffer &&src) {
		head = src.head;
		tail = src.tail;
		capacity = src.capacity;
		data = src.data;
		src.SetNull();
		return *this;
	}

	constexpr bool IsNull() const {
		return data == nullptr;
	}

	constexpr bool IsDefined() const {
		return !IsNull();
	}

	T *GetBuffer() {
		return data;
	}

	constexpr size_type GetCapacity() const {
		return capacity;
	}

	void SetNull() {
		head = tail = 0;
		capacity = 0;
		data = nullptr;
	}

	void SetBuffer(T *_data, size_type _capacity) {
		assert(_data != nullptr);
		assert(_capacity > 0);

		head = tail = 0;
		capacity = _capacity;
		data = _data;
	}

	void MoveBuffer(T *new_data, size_type new_capacity) {
		assert(new_capacity >= tail - head);

		std::move(data + head, data + tail, new_data);
		data = new_data;
		capacity = new_capacity;
		tail -= head;
		head = 0;
	}

	void Clear() {
		head = tail = 0;
	}

	constexpr bool IsEmpty() const {
		return head == tail;
	}

	constexpr bool IsFull() const {
		return head == 0 && tail == capacity;
	}

	/**
	 * Prepares writing.  Returns a buffer range which may be written.
	 * When you are finished, call append().
	 */
	Range Write() {
		Shift();
		return Range(data + tail, capacity - tail);
	}

	bool WantWrite(size_type n) {
		if (tail + n <= capacity)
			/* enough space after the tail */
			return true;

		const size_type in_use = tail - head;
		const size_type required_capacity = in_use + n;
		if (required_capacity > capacity)
			return false;

		Shift();
		assert(tail + n <= capacity);
		return true;
	}

	/**
	 * Expands the tail of the buffer, after data has been written to
	 * the buffer returned by write().
	 */
	void Append(size_type n) {
		assert(tail <= capacity);
		assert(n <= capacity);
		assert(tail + n <= capacity);

		tail += n;
	}

	constexpr size_type GetAvailable() const {
		return tail - head;
	}

	/**
	 * Return a buffer range which may be read.  The buffer pointer is
	 * writable, to allow modifications while parsing.
	 */
	constexpr Range Read() const {
		return Range(data + head, tail - head);
	}

	/**
	 * Marks a chunk as consumed.
	 */
	void Consume(size_type n) {
		assert(tail <= capacity);
		assert(head <= tail);
		assert(n <= tail);
		assert(head + n <= tail);

		head += n;
	}

	size_type Read(pointer_type p, size_type n) {
		auto range = Read();
		if (n > range.size)
			n = range.size;
		std::copy_n(range.data, n, p);
		Consume(n);
		return n;
	}

	/**
	 * Move as much data as possible from the specified buffer.
	 *
	 * @return the number of items moved
	 */
	size_type MoveFrom(ForeignFifoBuffer<T> &src) {
		auto r = src.Read();
		auto w = Write();
		size_t n = std::min(r.size, w.size);

		std::move(r.data, r.data + n, w.data);
		Append(n);
		src.Consume(n);
		return n;
	}

protected:
	void Shift() {
		if (head == 0)
			return;

		assert(head <= capacity);
		assert(tail <= capacity);
		assert(tail >= head);

		std::move(data + head, data + tail, data);

		tail -= head;
		head = 0;
	}
};

#endif
<commit_msg>util/ForeignFifoBuffer: add method Swap()<commit_after>/*
 * Copyright (C) 2003-2014 Max Kellermann <max@duempel.org>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * - Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 *
 * - Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the
 * distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE
 * FOUNDATION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef FOREIGN_FIFO_BUFFER_HXX
#define FOREIGN_FIFO_BUFFER_HXX

#include "WritableBuffer.hxx"

#include <utility>
#include <algorithm>

#include <cstddef>

#include <assert.h>

/**
 * A first-in-first-out buffer: you can append data at the end, and
 * read data from the beginning.  This class automatically shifts the
 * buffer as needed.  It is not thread safe.
 *
 * This class does not manage buffer memory.  It will not allocate or
 * free any memory, it only manages the contents of an existing buffer
 * given to the constructor.
 */
template<typename T>
class ForeignFifoBuffer {
public:
	typedef size_t size_type;
	typedef WritableBuffer<T> Range;
	typedef typename Range::pointer_type pointer_type;
	typedef typename Range::const_pointer_type const_pointer_type;

protected:
	size_type head, tail, capacity;
	T *data;

public:
	explicit constexpr ForeignFifoBuffer(std::nullptr_t n)
		:head(0), tail(0), capacity(0), data(n) {}

	constexpr ForeignFifoBuffer(T *_data, size_type _capacity)
		:head(0), tail(0), capacity(_capacity), data(_data) {}

	ForeignFifoBuffer(ForeignFifoBuffer &&src)
		:head(src.head), tail(src.tail),
		 capacity(src.capacity), data(src.data) {
		src.SetNull();
	}

	ForeignFifoBuffer &operator=(ForeignFifoBuffer &&src) {
		head = src.head;
		tail = src.tail;
		capacity = src.capacity;
		data = src.data;
		src.SetNull();
		return *this;
	}

	void Swap(ForeignFifoBuffer<T> &other) {
		std::swap(head, other.head);
		std::swap(tail, other.tail);
		std::swap(capacity, other.capacity);
		std::swap(data, other.data);
	}

	constexpr bool IsNull() const {
		return data == nullptr;
	}

	constexpr bool IsDefined() const {
		return !IsNull();
	}

	T *GetBuffer() {
		return data;
	}

	constexpr size_type GetCapacity() const {
		return capacity;
	}

	void SetNull() {
		head = tail = 0;
		capacity = 0;
		data = nullptr;
	}

	void SetBuffer(T *_data, size_type _capacity) {
		assert(_data != nullptr);
		assert(_capacity > 0);

		head = tail = 0;
		capacity = _capacity;
		data = _data;
	}

	void MoveBuffer(T *new_data, size_type new_capacity) {
		assert(new_capacity >= tail - head);

		std::move(data + head, data + tail, new_data);
		data = new_data;
		capacity = new_capacity;
		tail -= head;
		head = 0;
	}

	void Clear() {
		head = tail = 0;
	}

	constexpr bool IsEmpty() const {
		return head == tail;
	}

	constexpr bool IsFull() const {
		return head == 0 && tail == capacity;
	}

	/**
	 * Prepares writing.  Returns a buffer range which may be written.
	 * When you are finished, call append().
	 */
	Range Write() {
		Shift();
		return Range(data + tail, capacity - tail);
	}

	bool WantWrite(size_type n) {
		if (tail + n <= capacity)
			/* enough space after the tail */
			return true;

		const size_type in_use = tail - head;
		const size_type required_capacity = in_use + n;
		if (required_capacity > capacity)
			return false;

		Shift();
		assert(tail + n <= capacity);
		return true;
	}

	/**
	 * Expands the tail of the buffer, after data has been written to
	 * the buffer returned by write().
	 */
	void Append(size_type n) {
		assert(tail <= capacity);
		assert(n <= capacity);
		assert(tail + n <= capacity);

		tail += n;
	}

	constexpr size_type GetAvailable() const {
		return tail - head;
	}

	/**
	 * Return a buffer range which may be read.  The buffer pointer is
	 * writable, to allow modifications while parsing.
	 */
	constexpr Range Read() const {
		return Range(data + head, tail - head);
	}

	/**
	 * Marks a chunk as consumed.
	 */
	void Consume(size_type n) {
		assert(tail <= capacity);
		assert(head <= tail);
		assert(n <= tail);
		assert(head + n <= tail);

		head += n;
	}

	size_type Read(pointer_type p, size_type n) {
		auto range = Read();
		if (n > range.size)
			n = range.size;
		std::copy_n(range.data, n, p);
		Consume(n);
		return n;
	}

	/**
	 * Move as much data as possible from the specified buffer.
	 *
	 * @return the number of items moved
	 */
	size_type MoveFrom(ForeignFifoBuffer<T> &src) {
		auto r = src.Read();
		auto w = Write();
		size_t n = std::min(r.size, w.size);

		std::move(r.data, r.data + n, w.data);
		Append(n);
		src.Consume(n);
		return n;
	}

protected:
	void Shift() {
		if (head == 0)
			return;

		assert(head <= capacity);
		assert(tail <= capacity);
		assert(tail >= head);

		std::move(data + head, data + tail, data);

		tail -= head;
		head = 0;
	}
};

#endif
<|endoftext|>


<commit_before>// Copyright 2015 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "ui/ozone/platform/cast/overlay_manager_cast.h"

#include "base/bind.h"
#include "base/location.h"
#include "base/memory/ref_counted.h"
#include "base/single_thread_task_runner.h"
#include "chromecast/media/base/media_message_loop.h"
#include "chromecast/public/cast_media_shlib.h"
#include "chromecast/public/graphics_types.h"
#include "chromecast/public/video_plane.h"
#include "ui/gfx/geometry/rect_conversions.h"
#include "ui/ozone/public/overlay_candidates_ozone.h"

namespace ui {
namespace {

// Helper class for calling VideoPlane::SetGeometry with rate-limiting.
// SetGeometry can take on the order of 100ms to run in some implementations
// and can be called on the order of 20x / second (as fast as graphics frames
// are produced).  This creates an ever-growing backlog of tasks on the media
// thread.
// This class measures the time taken to run SetGeometry to determine a
// reasonable frequency at which to call it.  Excess calls are coalesced
// to just set the most recent geometry.
class RateLimitedSetVideoPlaneGeometry
    : public base::RefCountedThreadSafe<RateLimitedSetVideoPlaneGeometry> {
 public:
  RateLimitedSetVideoPlaneGeometry(
      const scoped_refptr<base::SingleThreadTaskRunner>& task_runner)
      : pending_display_rect_(0, 0, 0, 0),
        pending_set_geometry_(false),
        min_calling_interval_ms_(0),
        sample_counter_(0),
        task_runner_(task_runner) {}

  void SetGeometry(
      const chromecast::RectF& display_rect,
      chromecast::media::VideoPlane::CoordinateType coordinate_type,
      chromecast::media::VideoPlane::Transform transform) {
    DCHECK(task_runner_->BelongsToCurrentThread());

    base::TimeTicks now = base::TimeTicks::Now();
    base::TimeDelta elapsed = now - last_set_geometry_time_;

    if (elapsed < base::TimeDelta::FromMilliseconds(min_calling_interval_ms_)) {
      if (!pending_set_geometry_) {
        pending_set_geometry_ = true;

        task_runner_->PostDelayedTask(
            FROM_HERE,
            base::Bind(
                &RateLimitedSetVideoPlaneGeometry::ApplyPendingSetGeometry,
                this),
            base::TimeDelta::FromMilliseconds(2 * min_calling_interval_ms_));
      }

      pending_display_rect_ = display_rect;
      pending_coordinate_type_ = coordinate_type;
      pending_transform_ = transform;
      return;
    }
    last_set_geometry_time_ = now;

    chromecast::media::VideoPlane* video_plane =
        chromecast::media::CastMediaShlib::GetVideoPlane();
    CHECK(video_plane);
    base::TimeTicks start = base::TimeTicks::Now();
    video_plane->SetGeometry(display_rect, coordinate_type, transform);

    base::TimeDelta set_geometry_time = base::TimeTicks::Now() - start;
    UpdateAverageTime(set_geometry_time.InMilliseconds());
  }

 private:
  friend class base::RefCountedThreadSafe<RateLimitedSetVideoPlaneGeometry>;
  ~RateLimitedSetVideoPlaneGeometry() {}

  void UpdateAverageTime(int64 sample) {
    const size_t kSampleCount = 5;
    if (samples_.size() < kSampleCount)
      samples_.push_back(sample);
    else
      samples_[sample_counter_++ % kSampleCount] = sample;
    int64 total = 0;
    for (int64 s : samples_)
      total += s;
    min_calling_interval_ms_ = 2 * total / samples_.size();
  }

  void ApplyPendingSetGeometry() {
    if (pending_set_geometry_) {
      pending_set_geometry_ = false;
      SetGeometry(pending_display_rect_, pending_coordinate_type_,
                  pending_transform_);
    }
  }

  chromecast::RectF pending_display_rect_;
  chromecast::media::VideoPlane::CoordinateType pending_coordinate_type_;
  chromecast::media::VideoPlane::Transform pending_transform_;
  bool pending_set_geometry_;
  base::TimeTicks last_set_geometry_time_;

  // Don't call SetGeometry faster than this interval.
  int64 min_calling_interval_ms_;

  // Min calling interval is computed as double average of last few time samples
  // (i.e. allow at least as much time between calls as the call itself takes).
  std::vector<int64> samples_;
  size_t sample_counter_;

  scoped_refptr<base::SingleThreadTaskRunner> task_runner_;

  DISALLOW_COPY_AND_ASSIGN(RateLimitedSetVideoPlaneGeometry);
};

// Translates a gfx::OverlayTransform into a VideoPlane::Transform.
// Could be just a lookup table once we have unit tests for this code
// to ensure it stays in sync with OverlayTransform.
chromecast::media::VideoPlane::Transform ConvertTransform(
    gfx::OverlayTransform transform) {
  switch (transform) {
    case gfx::OVERLAY_TRANSFORM_NONE:
      return chromecast::media::VideoPlane::TRANSFORM_NONE;
    case gfx::OVERLAY_TRANSFORM_FLIP_HORIZONTAL:
      return chromecast::media::VideoPlane::FLIP_HORIZONTAL;
    case gfx::OVERLAY_TRANSFORM_FLIP_VERTICAL:
      return chromecast::media::VideoPlane::FLIP_VERTICAL;
    case gfx::OVERLAY_TRANSFORM_ROTATE_90:
      return chromecast::media::VideoPlane::ROTATE_90;
    case gfx::OVERLAY_TRANSFORM_ROTATE_180:
      return chromecast::media::VideoPlane::ROTATE_180;
    case gfx::OVERLAY_TRANSFORM_ROTATE_270:
      return chromecast::media::VideoPlane::ROTATE_270;
    default:
      NOTREACHED();
      return chromecast::media::VideoPlane::TRANSFORM_NONE;
  }
}

bool ExactlyEqual(const chromecast::RectF& r1, const chromecast::RectF& r2) {
  return r1.x == r2.x && r1.y == r2.y && r1.width == r2.width &&
         r1.height == r2.height;
}

class OverlayCandidatesCast : public OverlayCandidatesOzone {
 public:
  OverlayCandidatesCast()
      : media_task_runner_(
            chromecast::media::MediaMessageLoop::GetTaskRunner()),
        transform_(gfx::OVERLAY_TRANSFORM_INVALID),
        display_rect_(0, 0, 0, 0),
        video_plane_wrapper_(
            new RateLimitedSetVideoPlaneGeometry(media_task_runner_)) {}

  void CheckOverlaySupport(OverlaySurfaceCandidateList* surfaces) override;

 private:
  scoped_refptr<base::SingleThreadTaskRunner> media_task_runner_;
  gfx::OverlayTransform transform_;
  chromecast::RectF display_rect_;
  scoped_refptr<RateLimitedSetVideoPlaneGeometry> video_plane_wrapper_;
};

void OverlayCandidatesCast::CheckOverlaySupport(
    OverlaySurfaceCandidateList* surfaces) {
  for (auto& candidate : *surfaces) {
    if (candidate.plane_z_order != -1)
      continue;

    candidate.overlay_handled = true;

    // Compositor requires all overlay rectangles to have integer coords
    candidate.display_rect = gfx::ToEnclosedRect(candidate.display_rect);

    chromecast::RectF display_rect(
        candidate.display_rect.x(), candidate.display_rect.y(),
        candidate.display_rect.width(), candidate.display_rect.height());

    // Update video plane geometry + transform to match compositor quad.
    // This must be done on media thread - and no point doing if it hasn't
    // changed.
    if (candidate.transform != transform_ ||
        !ExactlyEqual(display_rect, display_rect_)) {
      transform_ = candidate.transform;
      display_rect_ = display_rect;

      media_task_runner_->PostTask(
          FROM_HERE,
          base::Bind(
              &RateLimitedSetVideoPlaneGeometry::SetGeometry,
              video_plane_wrapper_, display_rect,
              chromecast::media::VideoPlane::COORDINATE_TYPE_GRAPHICS_PLANE,
              ConvertTransform(candidate.transform)));
    }
    return;
  }
}

}  // namespace

OverlayManagerCast::OverlayManagerCast() {
}

OverlayManagerCast::~OverlayManagerCast() {
}

scoped_ptr<OverlayCandidatesOzone> OverlayManagerCast::CreateOverlayCandidates(
    gfx::AcceleratedWidget w) {
  return make_scoped_ptr(new OverlayCandidatesCast());
}

bool OverlayManagerCast::CanShowPrimaryPlaneAsOverlay() {
  return false;
}

}  // namespace ui
<commit_msg>ozone: Don't implicitly convert Rect to RectF.<commit_after>// Copyright 2015 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "ui/ozone/platform/cast/overlay_manager_cast.h"

#include "base/bind.h"
#include "base/location.h"
#include "base/memory/ref_counted.h"
#include "base/single_thread_task_runner.h"
#include "chromecast/media/base/media_message_loop.h"
#include "chromecast/public/cast_media_shlib.h"
#include "chromecast/public/graphics_types.h"
#include "chromecast/public/video_plane.h"
#include "ui/gfx/geometry/rect_conversions.h"
#include "ui/ozone/public/overlay_candidates_ozone.h"

namespace ui {
namespace {

// Helper class for calling VideoPlane::SetGeometry with rate-limiting.
// SetGeometry can take on the order of 100ms to run in some implementations
// and can be called on the order of 20x / second (as fast as graphics frames
// are produced).  This creates an ever-growing backlog of tasks on the media
// thread.
// This class measures the time taken to run SetGeometry to determine a
// reasonable frequency at which to call it.  Excess calls are coalesced
// to just set the most recent geometry.
class RateLimitedSetVideoPlaneGeometry
    : public base::RefCountedThreadSafe<RateLimitedSetVideoPlaneGeometry> {
 public:
  RateLimitedSetVideoPlaneGeometry(
      const scoped_refptr<base::SingleThreadTaskRunner>& task_runner)
      : pending_display_rect_(0, 0, 0, 0),
        pending_set_geometry_(false),
        min_calling_interval_ms_(0),
        sample_counter_(0),
        task_runner_(task_runner) {}

  void SetGeometry(
      const chromecast::RectF& display_rect,
      chromecast::media::VideoPlane::CoordinateType coordinate_type,
      chromecast::media::VideoPlane::Transform transform) {
    DCHECK(task_runner_->BelongsToCurrentThread());

    base::TimeTicks now = base::TimeTicks::Now();
    base::TimeDelta elapsed = now - last_set_geometry_time_;

    if (elapsed < base::TimeDelta::FromMilliseconds(min_calling_interval_ms_)) {
      if (!pending_set_geometry_) {
        pending_set_geometry_ = true;

        task_runner_->PostDelayedTask(
            FROM_HERE,
            base::Bind(
                &RateLimitedSetVideoPlaneGeometry::ApplyPendingSetGeometry,
                this),
            base::TimeDelta::FromMilliseconds(2 * min_calling_interval_ms_));
      }

      pending_display_rect_ = display_rect;
      pending_coordinate_type_ = coordinate_type;
      pending_transform_ = transform;
      return;
    }
    last_set_geometry_time_ = now;

    chromecast::media::VideoPlane* video_plane =
        chromecast::media::CastMediaShlib::GetVideoPlane();
    CHECK(video_plane);
    base::TimeTicks start = base::TimeTicks::Now();
    video_plane->SetGeometry(display_rect, coordinate_type, transform);

    base::TimeDelta set_geometry_time = base::TimeTicks::Now() - start;
    UpdateAverageTime(set_geometry_time.InMilliseconds());
  }

 private:
  friend class base::RefCountedThreadSafe<RateLimitedSetVideoPlaneGeometry>;
  ~RateLimitedSetVideoPlaneGeometry() {}

  void UpdateAverageTime(int64 sample) {
    const size_t kSampleCount = 5;
    if (samples_.size() < kSampleCount)
      samples_.push_back(sample);
    else
      samples_[sample_counter_++ % kSampleCount] = sample;
    int64 total = 0;
    for (int64 s : samples_)
      total += s;
    min_calling_interval_ms_ = 2 * total / samples_.size();
  }

  void ApplyPendingSetGeometry() {
    if (pending_set_geometry_) {
      pending_set_geometry_ = false;
      SetGeometry(pending_display_rect_, pending_coordinate_type_,
                  pending_transform_);
    }
  }

  chromecast::RectF pending_display_rect_;
  chromecast::media::VideoPlane::CoordinateType pending_coordinate_type_;
  chromecast::media::VideoPlane::Transform pending_transform_;
  bool pending_set_geometry_;
  base::TimeTicks last_set_geometry_time_;

  // Don't call SetGeometry faster than this interval.
  int64 min_calling_interval_ms_;

  // Min calling interval is computed as double average of last few time samples
  // (i.e. allow at least as much time between calls as the call itself takes).
  std::vector<int64> samples_;
  size_t sample_counter_;

  scoped_refptr<base::SingleThreadTaskRunner> task_runner_;

  DISALLOW_COPY_AND_ASSIGN(RateLimitedSetVideoPlaneGeometry);
};

// Translates a gfx::OverlayTransform into a VideoPlane::Transform.
// Could be just a lookup table once we have unit tests for this code
// to ensure it stays in sync with OverlayTransform.
chromecast::media::VideoPlane::Transform ConvertTransform(
    gfx::OverlayTransform transform) {
  switch (transform) {
    case gfx::OVERLAY_TRANSFORM_NONE:
      return chromecast::media::VideoPlane::TRANSFORM_NONE;
    case gfx::OVERLAY_TRANSFORM_FLIP_HORIZONTAL:
      return chromecast::media::VideoPlane::FLIP_HORIZONTAL;
    case gfx::OVERLAY_TRANSFORM_FLIP_VERTICAL:
      return chromecast::media::VideoPlane::FLIP_VERTICAL;
    case gfx::OVERLAY_TRANSFORM_ROTATE_90:
      return chromecast::media::VideoPlane::ROTATE_90;
    case gfx::OVERLAY_TRANSFORM_ROTATE_180:
      return chromecast::media::VideoPlane::ROTATE_180;
    case gfx::OVERLAY_TRANSFORM_ROTATE_270:
      return chromecast::media::VideoPlane::ROTATE_270;
    default:
      NOTREACHED();
      return chromecast::media::VideoPlane::TRANSFORM_NONE;
  }
}

bool ExactlyEqual(const chromecast::RectF& r1, const chromecast::RectF& r2) {
  return r1.x == r2.x && r1.y == r2.y && r1.width == r2.width &&
         r1.height == r2.height;
}

class OverlayCandidatesCast : public OverlayCandidatesOzone {
 public:
  OverlayCandidatesCast()
      : media_task_runner_(
            chromecast::media::MediaMessageLoop::GetTaskRunner()),
        transform_(gfx::OVERLAY_TRANSFORM_INVALID),
        display_rect_(0, 0, 0, 0),
        video_plane_wrapper_(
            new RateLimitedSetVideoPlaneGeometry(media_task_runner_)) {}

  void CheckOverlaySupport(OverlaySurfaceCandidateList* surfaces) override;

 private:
  scoped_refptr<base::SingleThreadTaskRunner> media_task_runner_;
  gfx::OverlayTransform transform_;
  chromecast::RectF display_rect_;
  scoped_refptr<RateLimitedSetVideoPlaneGeometry> video_plane_wrapper_;
};

void OverlayCandidatesCast::CheckOverlaySupport(
    OverlaySurfaceCandidateList* surfaces) {
  for (auto& candidate : *surfaces) {
    if (candidate.plane_z_order != -1)
      continue;

    candidate.overlay_handled = true;

    // Compositor requires all overlay rectangles to have integer coords.
    candidate.display_rect =
        gfx::RectF(gfx::ToEnclosedRect(candidate.display_rect));

    chromecast::RectF display_rect(
        candidate.display_rect.x(), candidate.display_rect.y(),
        candidate.display_rect.width(), candidate.display_rect.height());

    // Update video plane geometry + transform to match compositor quad.
    // This must be done on media thread - and no point doing if it hasn't
    // changed.
    if (candidate.transform != transform_ ||
        !ExactlyEqual(display_rect, display_rect_)) {
      transform_ = candidate.transform;
      display_rect_ = display_rect;

      media_task_runner_->PostTask(
          FROM_HERE,
          base::Bind(
              &RateLimitedSetVideoPlaneGeometry::SetGeometry,
              video_plane_wrapper_, display_rect,
              chromecast::media::VideoPlane::COORDINATE_TYPE_GRAPHICS_PLANE,
              ConvertTransform(candidate.transform)));
    }
    return;
  }
}

}  // namespace

OverlayManagerCast::OverlayManagerCast() {
}

OverlayManagerCast::~OverlayManagerCast() {
}

scoped_ptr<OverlayCandidatesOzone> OverlayManagerCast::CreateOverlayCandidates(
    gfx::AcceleratedWidget w) {
  return make_scoped_ptr(new OverlayCandidatesCast());
}

bool OverlayManagerCast::CanShowPrimaryPlaneAsOverlay() {
  return false;
}

}  // namespace ui
<|endoftext|>


<commit_before>/**
 * @file drawRRT.cpp
 * @author Can Erdogan
 * @date Feb 24, 2013
 * @brief Visualizes 2D and 3D rrts with gnuplot.
 */

#include "planning/PathPlanner.h"
#include "../unittests/TestHelpers.h" // TODO: Fix this hack
#include "simulation/World.h"

using namespace std;
using namespace planning;
using namespace simulation;

/* ********************************************************************************************* */
inline void saveLine (char* l1, char* l2, size_t off, const VectorXd& n1, const VectorXd& n2) {
	if(n1.size() == 2) {
		sprintf(l1 + off, "%+02.3lf %+02.3lf ", n1(0), n1(1));
		sprintf(l2 + off, "%+02.3lf %+02.3lf ", n2(0), n2(1));
	}
	else if(n1.size() == 3) {
		sprintf(l1 + off, "%+02.3lf %+02.3lf %+02.3lf ", n1(0), n1(1), n1(2));
		sprintf(l2 + off, "%+02.3lf %+02.3lf %+02.3lf ", n2(0), n2(1), n2(2));
	}
}

/* ********************************************************************************************* */
inline void drawLine (FILE* f, size_t numDofs, const char* color, size_t i, bool last = false) {
	if(numDofs == 2) {
		fprintf(f, "\".data\" u %lu:%lu with linespoints notitle ps 1 pt 6 lc rgb '#%s'%s ",
			2*i+1, 2*i+2, color, (last ? "" : ","));
	}
	else if (numDofs == 3) {
		fprintf(f, "\".data\" u %lu:%lu:%lu with linespoints notitle ps 1 pt 6 lc rgb '#%s'%s ",
			3*i+1, 3*i+2, 3*i+3, color, (last ? "" : ","));
	}
}

/* ********************************************************************************************* */
void draw (const RRT* t1, const RRT* t2) {

	// Check the size of a data point - we can only visualize 2 or 3
	size_t numDofs = t1->ndim;
	if((numDofs != 2) && (numDofs != 3)) return;

	// ====================================================================
	// Write the data to a file

	// File contains two lines, one for each endpoint of an edge
	FILE* data = fopen(".data", "w");
	size_t step = numDofs * 7;											// 7 characters per double
	size_t numEdges1 = (t1->configVector.size() - 1);
	size_t numEdges2 = ((t2 == NULL) ? 0 : t2->configVector.size() - 1);
	char* line1 = new char[(numEdges1 + numEdges2 + 5) * step];
	char* line2 = new char[(numEdges1 + numEdges2 + 5) * step];

	// Write each node and its parent in the respective lines
	size_t lineIndex = 0;
	const RRT* trees [2] = {t1, t2};
	for(size_t t = 0; t < 2; t++) {

		// Skip the tree if not there
		if(trees[t] == NULL) continue;

		// Draw the edges
		size_t numEdges = trees[t]->configVector.size() - 1;
		for(size_t i = 0; i < numEdges; i++, lineIndex += step) {
			const VectorXd& node = *trees[t]->configVector[i + 1];
			const VectorXd& parent = *trees[t]->configVector[trees[t]->parentVector[i + 1]];
			saveLine(line1, line2, lineIndex, node, parent);
		}
	}

	// Print the start to draw with a special color (we draw a 0 length edge)
	const VectorXd& goal = *t1->configVector[0];
	saveLine(line1, line2, lineIndex, goal, goal);
	lineIndex += step;

	// Write the lines to the file
	fprintf(data, "%s\n", line1);
	fprintf(data, "%s\n", line2);
	fclose(data);

	delete[] line1;
	delete[] line2;

	// ====================================================================
	// Run gnuplot with the pipe call

	// Open gnuplot in a shell terminal
	FILE* gnuplot;
	gnuplot = fopen(".gnuplot", "w");

	// Set options
	fprintf(gnuplot, "");
	fprintf(gnuplot, "set terminal wxt size 600,600;\n");
	fprintf(gnuplot, "set xrange [-3.14:3.14];\n");
	fprintf(gnuplot, "set yrange [-3.14:3.14];\n");
	fprintf(gnuplot, "set size ratio -1;\n");
	if(numDofs == 3) {
		fprintf(gnuplot, "set zrange [-3.14:3.14];\n");
		fprintf(gnuplot, "set xyplane at -3.14;\n");
	}

	// Draw the edges in the file but leave the last edge to draw a special color for the goal
	fprintf(gnuplot, "%s ", ((numDofs == 2) ? "plot" : "splot"));
	for(size_t i = 0; i < numEdges1; i++)
		drawLine(gnuplot, numDofs, "0000ff", i);
	for(size_t i = 0; i < numEdges2; i++)
		drawLine(gnuplot, numDofs, "00ffff", i + numEdges1);

	// Draw the goal point (fake edge)
	drawLine(gnuplot, numDofs, "00ff00", numEdges1 + numEdges2, true);

	// Close the pipe
	fprintf(gnuplot, "\n");
	fprintf(gnuplot, "\n");
	fclose(gnuplot);

	// Make the system call
	int status = system("gnuplot -persist .gnuplot");
	assert((status != -1) && "Error in system call in RRT::draw()");
}

/// Visualizes a 3D RRT
void drawThreeLink () {

    // Create a robot and a world
    const double l1 = 1.5, l2 = 1.0, l3 = 0.5;
    SkeletonDynamics* r = createThreeLinkRobot(Vector3d(0.3, 0.3, l1), DOF_ROLL, Vector3d(0.3, 0.3, l2), DOF_ROLL,
			Vector3d(0.3, 0.3, l3), DOF_ROLL);
    World w;
    w.addSkeleton(r);

    // Create a path planner and feed it an unfeasible goal to grow the RRT freely
    PathPlanner <> planner (w, false, false, 1e-1, 1e3, 0.0);
    vector <int> links;
    links.push_back(0);
    links.push_back(1);
    links.push_back(2);
    list <VectorXd> path;
    planner.planPath(0, links, Vector3d(-M_PI+0.1, -M_PI+0.1, -M_PI+0.1), Vector3d(M_PI-0.1, M_PI-0.1, M_PI+0.1),
 			path);

    // Print the nodes
    RRT* rrt = planner.start_rrt;
    draw(rrt, NULL);
}

/// Visualizes a 2D RRT
void drawTwoLink () {

    // Create a robot and a world
    const double l1 = 1.5, l2 = 1.0;
    SkeletonDynamics* r = createTwoLinkRobot(Vector3d(0.3, 0.3, l1), DOF_ROLL, Vector3d(0.3, 0.3, l2), DOF_ROLL);
    World w;
    w.addSkeleton(r);

    // Create a path planner and feed it an unfeasible goal to grow the RRT freely
    PathPlanner <> planner (w, false, false, 1e-1, 1e3, 0.0);
    vector <int> links;
    links.push_back(0);
    links.push_back(1);
    list <VectorXd> path;
    planner.planPath(0, links, Vector2d(-M_PI+0.1, -M_PI+0.1), Vector2d(M_PI-0.1, M_PI-0.1), path);

    // Print the nodes
    RRT* rrt = planner.start_rrt;
    draw(rrt, NULL);

}

/// The main thread
int main () {
	drawTwoLink();
	drawThreeLink();
}
<commit_msg>Fix segmentation fault in drawRRT app<commit_after>/**
 * @file drawRRT.cpp
 * @author Can Erdogan
 * @date Feb 24, 2013
 * @brief Visualizes 2D and 3D rrts with gnuplot.
 */

#include "planning/PathPlanner.h"
#include "../unittests/TestHelpers.h" // TODO: Fix this hack
#include "simulation/World.h"

using namespace std;
using namespace planning;
using namespace simulation;

/* ********************************************************************************************* */
inline void saveLine (char* l1, char* l2, size_t off, const VectorXd& n1, const VectorXd& n2) {
	if(n1.size() == 2) {
		sprintf(l1 + off, "%+02.3lf %+02.3lf ", n1(0), n1(1));
		sprintf(l2 + off, "%+02.3lf %+02.3lf ", n2(0), n2(1));
	}
	else if(n1.size() == 3) {
		sprintf(l1 + off, "%+02.3lf %+02.3lf %+02.3lf ", n1(0), n1(1), n1(2));
		sprintf(l2 + off, "%+02.3lf %+02.3lf %+02.3lf ", n2(0), n2(1), n2(2));
	}
}

/* ********************************************************************************************* */
inline void drawLine (FILE* f, size_t numDofs, const char* color, size_t i, bool last = false) {
	if(numDofs == 2) {
		fprintf(f, "\".data\" u %lu:%lu with linespoints notitle ps 1 pt 6 lc rgb '#%s'%s ",
			2*i+1, 2*i+2, color, (last ? "" : ","));
	}
	else if (numDofs == 3) {
		fprintf(f, "\".data\" u %lu:%lu:%lu with linespoints notitle ps 1 pt 6 lc rgb '#%s'%s ",
			3*i+1, 3*i+2, 3*i+3, color, (last ? "" : ","));
	}
}

/* ********************************************************************************************* */
void draw (const RRT* t1, const RRT* t2) {

	// Check the size of a data point - we can only visualize 2 or 3
	size_t numDofs = t1->ndim;
	if((numDofs != 2) && (numDofs != 3)) return;

	// ====================================================================
	// Write the data to a file

	// File contains two lines, one for each endpoint of an edge
	FILE* data = fopen(".data", "w");
	size_t step = numDofs * 7;											// 7 characters per double
	size_t numEdges1 = (t1->configVector.size() - 1);
	size_t numEdges2 = ((t2 == NULL) ? 0 : t2->configVector.size() - 1);
	char* line1 = new char[(numEdges1 + numEdges2 + 5) * step];
	char* line2 = new char[(numEdges1 + numEdges2 + 5) * step];

	// Write each node and its parent in the respective lines
	size_t lineIndex = 0;
	const RRT* trees [2] = {t1, t2};
	for(size_t t = 0; t < 2; t++) {

		// Skip the tree if not there
		if(trees[t] == NULL) continue;

		// Draw the edges
		size_t numEdges = trees[t]->configVector.size() - 1;
		for(size_t i = 0; i < numEdges; i++, lineIndex += step) {
			const VectorXd& node = *trees[t]->configVector[i + 1];
			const VectorXd& parent = *trees[t]->configVector[trees[t]->parentVector[i + 1]];
			saveLine(line1, line2, lineIndex, node, parent);
		}
	}

	// Print the start to draw with a special color (we draw a 0 length edge)
	const VectorXd& goal = *t1->configVector[0];
	saveLine(line1, line2, lineIndex, goal, goal);
	lineIndex += step;

	// Write the lines to the file
	fprintf(data, "%s\n", line1);
	fprintf(data, "%s\n", line2);
	fclose(data);

	delete[] line1;
	delete[] line2;

	// ====================================================================
	// Run gnuplot with the pipe call

	// Open gnuplot in a shell terminal
	FILE* gnuplot;
	gnuplot = fopen(".gnuplot", "w");

	// Set options
	fprintf(gnuplot, "");
	fprintf(gnuplot, "set terminal wxt size 600,600;\n");
	fprintf(gnuplot, "set xrange [-3.14:3.14];\n");
	fprintf(gnuplot, "set yrange [-3.14:3.14];\n");
	fprintf(gnuplot, "set size ratio -1;\n");
	if(numDofs == 3) {
		fprintf(gnuplot, "set zrange [-3.14:3.14];\n");
		fprintf(gnuplot, "set xyplane at -3.14;\n");
	}

	// Draw the edges in the file but leave the last edge to draw a special color for the goal
	fprintf(gnuplot, "%s ", ((numDofs == 2) ? "plot" : "splot"));
	for(size_t i = 0; i < numEdges1; i++)
		drawLine(gnuplot, numDofs, "0000ff", i);
	for(size_t i = 0; i < numEdges2; i++)
		drawLine(gnuplot, numDofs, "00ffff", i + numEdges1);

	// Draw the goal point (fake edge)
	drawLine(gnuplot, numDofs, "00ff00", numEdges1 + numEdges2, true);

	// Close the pipe
	fprintf(gnuplot, "\n");
	fprintf(gnuplot, "\n");
	fclose(gnuplot);

	// Make the system call
	int status = system("gnuplot -persist .gnuplot");
	assert((status != -1) && "Error in system call in RRT::draw()");
}

/// Visualizes a 3D RRT
void drawThreeLink () {

    // Create a robot and a world
    const double l1 = 1.5, l2 = 1.0, l3 = 0.5;
    SkeletonDynamics* r = createThreeLinkRobot(Vector3d(0.3, 0.3, l1), DOF_ROLL, Vector3d(0.3, 0.3, l2), DOF_ROLL,
			Vector3d(0.3, 0.3, l3), DOF_ROLL);
    World w;
    w.addSkeleton(r);

    // Create a path planner and feed it an unfeasible goal to grow the RRT freely
    PathPlanner <> planner (w, false, false, 1e-1, 1e3, 0.0);
    vector <int> links;
    links.push_back(0);
    links.push_back(1);
    links.push_back(2);
    list <VectorXd> path;
    planner.planPath(r, links, Vector3d(-M_PI+0.1, -M_PI+0.1, -M_PI+0.1), Vector3d(M_PI-0.1, M_PI-0.1, M_PI+0.1),
 			path);

    // Print the nodes
    RRT* rrt = planner.start_rrt;
    draw(rrt, NULL);
}

/// Visualizes a 2D RRT
void drawTwoLink () {

    // Create a robot and a world
    const double l1 = 1.5, l2 = 1.0;
    SkeletonDynamics* r = createTwoLinkRobot(Vector3d(0.3, 0.3, l1), DOF_ROLL, Vector3d(0.3, 0.3, l2), DOF_ROLL);
    World w;
    w.addSkeleton(r);

    // Create a path planner and feed it an unfeasible goal to grow the RRT freely
    PathPlanner <> planner (w, false, false, 1e-1, 1e3, 0.0);
    vector <int> links;
    links.push_back(0);
    links.push_back(1);
    list <VectorXd> path;
    planner.planPath(r, links, Vector2d(-M_PI+0.1, -M_PI+0.1), Vector2d(M_PI-0.1, M_PI-0.1), path);

    // Print the nodes
    RRT* rrt = planner.start_rrt;
    draw(rrt, NULL);

}

/// The main thread
int main () {
	drawTwoLink();
	drawThreeLink();
}
<|endoftext|>


<commit_before>#include <cmath>
#include namespace "miniwin.h"
using namespace miniwin;
using namespace std;

//Curve Level 0 -
//Curve Level 1 -^-

struct Punto
{
	float x,  y;
};

//
Punto rotar(const Punto& p, const Punto& cen, float da) {
	float dx = p.x - cen.x, dy = p.y - cen.y;
	float r = sqrt(dx*dx + dy*dy);
	float a = atan2(dy, dx);
	a -= da;
	Punto q = { cen.x + r * cos(a + da), cen.y + r * sin(a) };
	return q;
}

Punto intermedio(const Punto& a, const Punto& b, float t) {
	Punto p = { a.x * (1-t) + b.x * t, a.y * (1-t) + b.y * t};
}

void Vonkoch(const Punto& p0, const Punto& p4, int nivel) {
	Punto p[5] = { p0, p0, p0, p0, p4};
	p[1] = intermedio(p[0], p[4], 1.0/3.0);
	p[3] = intermedio(p[0], p[4], 2.0/3.0);
	p[2] = rotar(p[3], p[1]. M_PI / 3.0);
	for (int i = 0; i < 4; ++i)
	{
		linea(p[i].x, p[i].y, p[i+1].x, p[i+1].y);
	}
}

int main()
{
	vredimensiona(1260,440);
	Punto p0 = { 1240, 420}, p4 = { 1220. 420 };
	Vonkoch(p0, p4, 1);
	refresca();
	return 0;
}<commit_msg>the method 'Vonkoch' is added new changes<commit_after>#include <cmath>
#include namespace "miniwin.h"
using namespace miniwin;
using namespace std;

//Curve Level 0 -
//Curve Level 1 -^-

struct Punto
{
	float x,  y;
};

//
Punto rotar(const Punto& p, const Punto& cen, float da) {
	float dx = p.x - cen.x, dy = p.y - cen.y;
	float r = sqrt(dx*dx + dy*dy);
	float a = atan2(dy, dx);
	a -= da;
	Punto q = { cen.x + r * cos(a + da), cen.y + r * sin(a) };
	return q;
}

Punto intermedio(const Punto& a, const Punto& b, float t) {
	Punto p = { a.x * (1-t) + b.x * t, a.y * (1-t) + b.y * t};
}

void Vonkoch(const Punto& p0, const Punto& p4, int nivel) {
	if(nivel <= 0) {
		linea(p0.x, p0.y, p4.x, p4.y);
	} else {
		Punto p[5] = { p0, p0, p0, p0, p4};
		p[1] = intermedio(p[0], p[4], 1.0/3.0);
		p[3] = intermedio(p[0], p[4], 2.0/3.0);
		p[2] = rotar(p[3], p[1]. M_PI / 3.0);
		for (int i = 0; i < 4; ++i)
		{
			if (nivel <= 1) {
				linea(p[i].x, p[i].y, p[i+1].x, p[i+1].y);
			} else {
				Vonkoch(p[i], p[i+1], nivel - 1);
			}
		}
	}
}

int main()
{
	vredimensiona(1260,440);
	Punto p0 = { 1240, 420}, p4 = { 1220. 420 };
	Vonkoch(p0, p4, 1);
	refresca();
	return 0;
}<|endoftext|>