<commit_before>// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include "attributefactory.h"
#include "integerbase.h"
#include "floatbase.h"
#include "defines.h"
#include "singlestringattribute.h"
#include "singleboolattribute.h"
#include "singlestringpostattribute.hpp"
#include "singlenumericpostattribute.hpp"

#define INTPOSTING(T)   SingleValueNumericPostingAttribute< ENUM_ATTRIBUTE(IntegerAttributeTemplate<T>) >
#define FLOATPOSTING(T) SingleValueNumericPostingAttribute< ENUM_ATTRIBUTE(FloatingPointAttributeTemplate<T>) >

namespace search {

using attribute::BasicType;

AttributeVector::SP
AttributeFactory::createSingleFastSearch(stringref name, const Config & info)
{
    assert(info.collectionType().type() == attribute::CollectionType::SINGLE);
    assert(info.fastSearch());
    switch(info.basicType().type()) {
    case BasicType::BOOL:
        return std::make_shared<SingleBoolAttribute>(name, info.getGrowStrategy());
    case BasicType::UINT2:
    case BasicType::UINT4:
        break;
    case BasicType::INT8:
        return std::make_shared<INTPOSTING(int8_t)>(name, info);
    case BasicType::INT16:
        return std::make_shared<INTPOSTING(int16_t)>(name, info);
    case BasicType::INT32:
        return std::make_shared<INTPOSTING(int32_t)>(name, info);
    case BasicType::INT64:
        return std::make_shared<INTPOSTING(int64_t)>(name, info);
    case BasicType::FLOAT:
        return std::make_shared<FLOATPOSTING(float)>(name, info);
    case BasicType::DOUBLE:
        return std::make_shared<FLOATPOSTING(double)>(name, info);
    case BasicType::STRING:
        return std::make_shared<SingleValueStringPostingAttribute>(name, info);
    default:
        break;
    }
    return AttributeVector::SP();
}

}
<commit_msg>create DirectTensorAttribute for sparse tensors with fast-search<commit_after>// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include "attributefactory.h"
#include "integerbase.h"
#include "floatbase.h"
#include "defines.h"
#include "singlestringattribute.h"
#include "singleboolattribute.h"
#include "singlestringpostattribute.hpp"
#include "singlenumericpostattribute.hpp"
#include <vespa/searchlib/tensor/direct_tensor_attribute.h>

#define INTPOSTING(T)   SingleValueNumericPostingAttribute< ENUM_ATTRIBUTE(IntegerAttributeTemplate<T>) >
#define FLOATPOSTING(T) SingleValueNumericPostingAttribute< ENUM_ATTRIBUTE(FloatingPointAttributeTemplate<T>) >

namespace search {

using attribute::BasicType;

AttributeVector::SP
AttributeFactory::createSingleFastSearch(stringref name, const Config & info)
{
    assert(info.collectionType().type() == attribute::CollectionType::SINGLE);
    assert(info.fastSearch());
    switch(info.basicType().type()) {
    case BasicType::BOOL:
        return std::make_shared<SingleBoolAttribute>(name, info.getGrowStrategy());
    case BasicType::UINT2:
    case BasicType::UINT4:
        break;
    case BasicType::INT8:
        return std::make_shared<INTPOSTING(int8_t)>(name, info);
    case BasicType::INT16:
        return std::make_shared<INTPOSTING(int16_t)>(name, info);
    case BasicType::INT32:
        return std::make_shared<INTPOSTING(int32_t)>(name, info);
    case BasicType::INT64:
        return std::make_shared<INTPOSTING(int64_t)>(name, info);
    case BasicType::FLOAT:
        return std::make_shared<FLOATPOSTING(float)>(name, info);
    case BasicType::DOUBLE:
        return std::make_shared<FLOATPOSTING(double)>(name, info);
    case BasicType::STRING:
        return std::make_shared<SingleValueStringPostingAttribute>(name, info);
    case BasicType::TENSOR:
        if (info.tensorType().is_sparse()) {
            return std::make_shared<tensor::DirectTensorAttribute>(name, info);
        }
        break;
    default:
        break;
    }
    return AttributeVector::SP();
}

}
<|endoftext|>


<commit_before>#define GLOG_NO_ABBREVIATED_SEVERITIES
#undef TRACE_SYMPLECTIC_PARTITIONED_RUNGE_KUTTA_INTEGRATOR

#include <algorithm>

#include "benchmark/benchmark.h"
#include "integrators/symplectic_partitioned_runge_kutta_integrator.hpp"

using principia::integrators::SPRKIntegrator;

namespace principia {
namespace benchmarks {

namespace {

inline void compute_harmonic_oscillator_force(double const t,
                                              std::vector<double> const& q,
                                              std::vector<double>* result) {
  (*result)[0] = -q[0];
}

inline void compute_harmonice_oscillator_velocity(std::vector<double> const& p,
                                                  std::vector<double>* result) {
  (*result)[0] = p[0];
}

}  // namespace

void SolveHarmonicOscillator(benchmark::State* state,
                             double* q_error,
                             double* p_error) {
  SPRKIntegrator integrator;
  SPRKIntegrator::Parameters parameters;
  SPRKIntegrator::Solution solution;

  parameters.q0 = {1.0};
  parameters.p0 = {0.0};
  parameters.t0 = 0.0;
#ifdef _DEBUG
  parameters.tmax = 100.0;
#else
  parameters.tmax = 1000.0;
#endif
  parameters.Δt = 1.0E-4;
  parameters.coefficients = integrator.Order5Optimal();
  parameters.sampling_period = 1;
  integrator.Solve(&compute_harmonic_oscillator_force,
                    &compute_harmonice_oscillator_velocity,
                    parameters,
                    &solution);

  state->PauseTiming();
  *q_error = 0;
  *p_error = 0;
  for (size_t i = 0; i < solution.time.quantities.size(); ++i) {
    *q_error = std::max(*q_error,
                        std::abs(solution.position[0].quantities[i] -
                                 std::cos(solution.time.quantities[i])));
    *p_error = std::max(*p_error,
                        std::abs(solution.momentum[0].quantities[i] +
                                 std::sin(solution.time.quantities[i])));
  }
  state->ResumeTiming();
}

static void BM_SolveHarmonicOscillator(benchmark::State& state) {
  double q_error;
  double p_error;
  while (state.KeepRunning()) {
    SolveHarmonicOscillator(&state, &q_error, &p_error);
  }
  std::stringstream ss;
  ss << q_error << ", " << p_error;
  state.SetLabel(ss.str());
}
BENCHMARK(BM_SolveHarmonicOscillator);

}  // namespace benchmarks
}  // namespace principia

int main(int argc, const char* argv[]) {
  benchmark::Initialize(&argc, argv);

  benchmark::RunSpecifiedBenchmarks();
}
<commit_msg>Lint and warnings.<commit_after>#define GLOG_NO_ABBREVIATED_SEVERITIES
#undef TRACE_SYMPLECTIC_PARTITIONED_RUNGE_KUTTA_INTEGRATOR

#include <algorithm>
#include <vector>

#include "integrators/symplectic_partitioned_runge_kutta_integrator.hpp"
// Must come last to avoid conflicts when defining the CHECK macros.
#include "benchmark/benchmark.h"

using principia::integrators::SPRKIntegrator;

namespace principia {
namespace benchmarks {

namespace {

inline void compute_harmonic_oscillator_force(double const t,
                                              std::vector<double> const& q,
                                              std::vector<double>* result) {
  (*result)[0] = -q[0];
}

inline void compute_harmonic_oscillator_velocity(std::vector<double> const& p,
                                                  std::vector<double>* result) {
  (*result)[0] = p[0];
}

}  // namespace

void SolveHarmonicOscillator(benchmark::State* state,
                             double* q_error,
                             double* p_error) {
  SPRKIntegrator integrator;
  SPRKIntegrator::Parameters parameters;
  SPRKIntegrator::Solution solution;

  parameters.q0 = {1.0};
  parameters.p0 = {0.0};
  parameters.t0 = 0.0;
#ifdef _DEBUG
  parameters.tmax = 100.0;
#else
  parameters.tmax = 1000.0;
#endif
  parameters.Δt = 1.0E-4;
  parameters.coefficients = integrator.Order5Optimal();
  parameters.sampling_period = 1;
  integrator.Solve(&compute_harmonic_oscillator_force,
                   &compute_harmonic_oscillator_velocity,
                   parameters,
                   &solution);

  state->PauseTiming();
  *q_error = 0;
  *p_error = 0;
  for (size_t i = 0; i < solution.time.quantities.size(); ++i) {
    *q_error = std::max(*q_error,
                        std::abs(solution.position[0].quantities[i] -
                                 std::cos(solution.time.quantities[i])));
    *p_error = std::max(*p_error,
                        std::abs(solution.momentum[0].quantities[i] +
                                 std::sin(solution.time.quantities[i])));
  }
  state->ResumeTiming();
}

static void BM_SolveHarmonicOscillator(
    benchmark::State& state) {  // NOLINT(runtime/references)
  double q_error;
  double p_error;
  while (state.KeepRunning()) {
    SolveHarmonicOscillator(&state, &q_error, &p_error);
  }
  std::stringstream ss;
  ss << q_error << ", " << p_error;
  state.SetLabel(ss.str());
}
BENCHMARK(BM_SolveHarmonicOscillator);

}  // namespace benchmarks
}  // namespace principia

int main(int argc, const char* argv[]) {
  benchmark::Initialize(&argc, argv);

  benchmark::RunSpecifiedBenchmarks();
}
<|endoftext|>


<commit_before>#include <iostream>
#include <iomanip> // << fixed << setprecision(xxx)
#include <algorithm> // do { } while ( next_permutation(A, A+xxx) ) ;
#include <vector>
#include <string> // to_string(nnn) // substr(m, n) // stoi(nnn)
#include <complex>
#include <tuple> // get<n>(xxx)
#include <queue>
#include <stack>
#include <map> // if (M.find(key) != M.end()) { }
#include <set> // S.insert(M);
// if (S.find(key) != S.end()) { }
// for (auto it=S.begin(); it != S.end(); it++) { }
// auto it = S.lower_bound(M);
#include <random> // random_device rd; mt19937 mt(rd());
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib> // atoi(xxx)
using namespace std;

#define DEBUG 0 // change 0 -> 1 if we need debug.
// insert #if<tab> by my emacs. #if DEBUG == 1 ... #end

typedef long long ll;

// const int dx[4] = {1, 0, -1, 0};
// const int dy[4] = {0, 1, 0, -1};

// const int C = 1e6+10;
// const ll M = 1000000007;

const ll infty = 10000000000000007;

struct edge {
  int to;
  ll cap;
  int rev;
};

vector<edge> G[100010];
bool used[100010];

void add_edge(int from, int to, ll cap) {
  G[from].push_back((edge){to, cap, (int)G[to].size()});
  G[to].push_back((edge){from, 0, (int)G[from].size()-1});
}

ll dfs(int v, int t, ll f) {
  if (v == t) return f;
  used[v] = true;
  for (auto& e : G[v]) {
    if (!used[e.to] && e.cap > 0) {
      ll d = dfs(e.to, t, min(f, e.cap));
      if (d > 0) {
        e.cap -= d;
        G[e.to][e.rev].cap += d;
        return d;
      }
    }
  }
  return 0;
}

ll max_flow(int s, int t) {
  ll flow = 0;
  while (true) {
    fill(used, used+100010, false);
    ll f = dfs(s, t, infty);
    if (f == 0) return flow;
    flow += f;
  }
}

typedef tuple<ll, int> state; // kachi, bit

int N;
ll x[20];
ll c[20];
ll v[20];
vector<state> V[20];

int main () {
  cin >> N;
  for (auto i = 0; i < N; ++i) {
    cin >> x[i];
  }
  for (auto i = 0; i < N-1; ++i) {
    x[i+1] += x[i];
  }
  for (auto i = 0; i < N; ++i) {
    cin >> c[i];
  }
  for (auto i = 0; i < N; ++i) {
    cin >> v[i];
  }
  for (auto i = 0; i < (1 << N); ++i) {
    ll kakaku = 0;
    ll kachi = 0;
    for (auto j = 0; j < N; ++j) {
      if ((i >> j) & 1) {
        kakaku += c[j];
        kachi += v[j];
      }
    }
    for (auto j = 0; j < N; ++j) {
      if (kakaku <= x[j]) {
        V[j].push_back(make_tuple(kachi, i));
        break;
      }
    }
  }
  ll ans = 0;
  for (auto i = 0; i < N; ++i) {
    sort(V[i].begin(), V[i].end());
    reverse(V[i].begin(), V[i].end());
    int comb = (1 << (i+1)) - 1;
    while (comb < (1 << N)) {
      // ここで作業する
      for (auto x : V[i]) {
        int bit = get<1>(x);
        if ((comb & bit) > 0) {
          // ban
        } else {
          ans = max(ans, get<0>(x));
          break;
        }
      }
      // 以下処理。
      int tmpx = comb & (-comb);
      int tmpy = comb + tmpx;
      int tmpz = comb & (~(tmpy));
      comb = (((tmpz/tmpx) >> 1) | tmpy);
    }
  }
  cout << ans << endl;
}
<commit_msg>tried C.cpp to 'C'<commit_after>#include <iostream>
#include <iomanip> // << fixed << setprecision(xxx)
#include <algorithm> // do { } while ( next_permutation(A, A+xxx) ) ;
#include <vector>
#include <string> // to_string(nnn) // substr(m, n) // stoi(nnn)
#include <complex>
#include <tuple> // get<n>(xxx)
#include <queue>
#include <stack>
#include <map> // if (M.find(key) != M.end()) { }
#include <set> // S.insert(M);
// if (S.find(key) != S.end()) { }
// for (auto it=S.begin(); it != S.end(); it++) { }
// auto it = S.lower_bound(M);
#include <random> // random_device rd; mt19937 mt(rd());
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib> // atoi(xxx)
using namespace std;

#define DEBUG 0 // change 0 -> 1 if we need debug.
// insert #if<tab> by my emacs. #if DEBUG == 1 ... #end

typedef long long ll;

// const int dx[4] = {1, 0, -1, 0};
// const int dy[4] = {0, 1, 0, -1};

// const int C = 1e6+10;
// const ll M = 1000000007;

const ll infty = 10000000000000007;

struct edge {
  int to;
  ll cap;
  int rev;
};

vector<edge> G[100010];
bool used[100010];

void add_edge(int from, int to, ll cap) {
  G[from].push_back((edge){to, cap, (int)G[to].size()});
  G[to].push_back((edge){from, 0, (int)G[from].size()-1});
}

ll dfs(int v, int t, ll f) {
  if (v == t) return f;
  used[v] = true;
  for (auto& e : G[v]) {
    if (!used[e.to] && e.cap > 0) {
      ll d = dfs(e.to, t, min(f, e.cap));
      if (d > 0) {
        e.cap -= d;
        G[e.to][e.rev].cap += d;
        return d;
      }
    }
  }
  return 0;
}

ll max_flow(int s, int t) {
  ll flow = 0;
  while (true) {
    fill(used, used+100010, false);
    ll f = dfs(s, t, infty);
    if (f == 0) return flow;
    flow += f;
  }
}

typedef tuple<ll, int> state; // kachi, bit

int N;
ll x[20];
ll c[20];
ll v[20];
vector<state> V[20];

int main () {
  cin >> N;
  for (auto i = 0; i < N; ++i) {
    cin >> x[i];
  }
  for (auto i = 0; i < N-1; ++i) {
    x[i+1] += x[i];
  }
  for (auto i = 0; i < N; ++i) {
    cin >> c[i];
  }
  for (auto i = 0; i < N; ++i) {
    cin >> v[i];
  }
  for (auto i = 0; i < (1 << N); ++i) {
    ll kakaku = 0;
    ll kachi = 0;
    for (auto j = 0; j < N; ++j) {
      if ((i >> j) & 1) {
        kakaku += c[j];
        kachi += v[j];
      }
    }
    for (auto j = 0; j < N; ++j) {
      if (kakaku <= x[j]) {
        V[j].push_back(make_tuple(kachi, i));
        break;
      }
    }
  }
  ll ans = 0;
  for (auto i = 0; i < N; ++i) {
    sort(V[i].begin(), V[i].end());
    reverse(V[i].begin(), V[i].end());
    int comb = (1 << (i+1)) - 1;
    while (comb < (1 << N)) {
      // ここで作業する
      cerr << "comb = " << comb << endl;
      for (auto x : V[i]) {
        int bit = get<1>(x);
        if ((comb & bit) > 0) {
          // ban
          cerr << "bit = " << bit << " is banned." << endl;
        } else {
          cerr << "bit = " << bit << " is allowed. Get " << get<0>(x)
               << "." << endl;
          ans = max(ans, get<0>(x));
          break;
        }
      }
      // 以下処理。
      int tmpx = comb & (-comb);
      int tmpy = comb + tmpx;
      int tmpz = comb & (~(tmpy));
      comb = (((tmpz/tmpx) >> 1) | tmpy);
    }
  }
  cout << ans << endl;
}
<|endoftext|>


<commit_before>//============================================================================
// Name        : jaco_arm.cpp
// Author      : WPI, Clearpath Robotics
// Version     : 0.5
// Copyright   : BSD
// Description : A ROS driver for controlling the Kinova Jaco robotic manipulator arm
//============================================================================

#include "jaco_driver/jaco_arm.h"

namespace jaco
{

JacoArm::JacoArm(JacoComm &arm_comm, ros::NodeHandle &nh) : arm(arm_comm)
{
	std::string joint_velocity_topic, joint_angles_topic, cartesian_velocity_topic,
		tool_position_topic, set_finger_position_topic, finger_position_topic, joint_state_topic,
		set_joint_angle_topic;

	nh.param<std::string>("joint_velocity_topic", joint_velocity_topic, "joint_velocity");
	nh.param<std::string>("joint_angles_topic", joint_angles_topic, "joint_angles");
	nh.param<std::string>("cartesian_velocity_topic", cartesian_velocity_topic, "cartesian_velocity");
	nh.param<std::string>("tool_position_topic", tool_position_topic, "tool_position");
	nh.param<std::string>("set_finger_position_topic", set_finger_position_topic, "set_finger_position");
	nh.param<std::string>("finger_position_topic", finger_position_topic, "finger_position");
	nh.param<std::string>("joint_state_topic", joint_state_topic, "joint_state");
	nh.param<std::string>("set_joint_angle_topic", set_joint_angle_topic, "set_joint_angle");

	//Print out received topics
	ROS_DEBUG("Got Joint Velocity Topic Name: <%s>", joint_velocity_topic.c_str());
	ROS_DEBUG("Got Joint Angles Topic Name: <%s>", joint_angles_topic.c_str());
	ROS_DEBUG("Got Cartesian Velocity Topic Name: <%s>", cartesian_velocity_topic.c_str());
	ROS_DEBUG("Got Tool Position Topic Name: <%s>", tool_position_topic.c_str());
	ROS_DEBUG("Got Set Finger Position Topic Name: <%s>", set_finger_position_topic.c_str());
	ROS_DEBUG("Got Finger Position Topic Name: <%s>", finger_position_topic.c_str());
	ROS_DEBUG("Got Joint State Topic Name: <%s>", joint_state_topic.c_str());
	ROS_DEBUG("Got Set Joint Angle Topic Name: <%s>", set_joint_angle_topic.c_str());

	ROS_INFO("Starting Up Jaco Arm Controller...");

	previous_state = 0;

	/* Set up Services */
	stop_service = nh.advertiseService("stop", &JacoArm::StopSRV, this);
	start_service = nh.advertiseService("start", &JacoArm::StartSRV, this);
	homing_service = nh.advertiseService("home_arm", &JacoArm::HomeArmSRV, this);

	/* Set Default Configuration */

	// API->RestoreFactoryDefault(); // uncomment comment ONLY if you want to lose your settings on each launch.

	ClientConfigurations configuration;
	arm.GetConfig(configuration);
	arm.PrintConfig(configuration);

	last_update_time = ros::Time::now();
	update_time = ros::Duration(5.0);

	/* Storing arm in home position */

	/* Set up Publishers */
	JointAngles_pub = nh.advertise<jaco_driver::JointAngles>(joint_angles_topic, 2);
	JointState_pub = nh.advertise<sensor_msgs::JointState>(joint_state_topic, 2);
	ToolPosition_pub = nh.advertise<geometry_msgs::PoseStamped>(tool_position_topic, 2);
	FingerPosition_pub = nh.advertise<jaco_driver::FingerPosition>(finger_position_topic, 2);

	/* Set up Subscribers*/
	JointVelocity_sub = nh.subscribe(joint_velocity_topic, 1, &JacoArm::VelocityMSG, this);
	CartesianVelocity_sub = nh.subscribe(cartesian_velocity_topic, 1, &JacoArm::CartesianVelocityMSG, this);
	SetFingerPosition_sub = nh.subscribe(set_finger_position_topic, 1, &JacoArm::SetFingerPositionMSG, this);
	SetJoint_sub = nh.subscribe(set_joint_angle_topic, 1, &JacoArm::SetJointAnglesMSG, this);

	status_timer = nh.createTimer(ros::Duration(0.05), &JacoArm::StatusTimer, this);

	joint_vel_timer = nh.createTimer(ros::Duration(0.01), &JacoArm::JointVelTimer, this);
	joint_vel_timer.stop();
	joint_vel_timer_flag = false;
	cartesian_vel_timer = nh.createTimer(ros::Duration(0.01), &JacoArm::CartesianVelTimer, this);
	cartesian_vel_timer.stop();
	cartesian_vel_timer_flag = false;

	ROS_INFO("The Arm is ready to use.");

	TrajectoryPoint Jaco_Velocity;

	memset(&Jaco_Velocity, 0, sizeof(Jaco_Velocity)); //zero structure

	arm.SetCartesianVelocities(Jaco_Velocity.Position.CartesianPosition);
}

JacoArm::~JacoArm()
{
}

bool JacoArm::HomeArmSRV(jaco_driver::HomeArm::Request &req, jaco_driver::HomeArm::Response &res)
{
	arm.HomeArm();
	res.homearm_result = "JACO ARM HAS BEEN RETURNED HOME";

	return true;
}

/*!
 * \brief Receives ROS command messages and relays them to SetFingers.
 */
void JacoArm::SetFingerPositionMSG(const jaco_driver::FingerPositionConstPtr& finger_pos)
{
	if (!arm.Stopped())
	{
		FingersPosition Finger_Position;
		memset(&Finger_Position, 0, sizeof(Finger_Position)); //zero structure

		Finger_Position.Finger1 = finger_pos->Finger_1;
		Finger_Position.Finger2 = finger_pos->Finger_2;
		Finger_Position.Finger3 = finger_pos->Finger_3;

		arm.SetFingers(Finger_Position);
	}
}

/*!
 * \brief Receives ROS command messages and relays them to SetAngles.
 */
void JacoArm::SetJointAnglesMSG(const jaco_driver::JointAnglesConstPtr& angles)
{
	if (!arm.Stopped())
	{
		AngularInfo Joint_Position;
		memset(&Joint_Position, 0, sizeof(Joint_Position)); //zero structure

		Joint_Position.Actuator1 = angles->Angle_J1;
		Joint_Position.Actuator2 = angles->Angle_J2;
		Joint_Position.Actuator3 = angles->Angle_J3;
		Joint_Position.Actuator4 = angles->Angle_J4;
		Joint_Position.Actuator5 = angles->Angle_J5;
		Joint_Position.Actuator6 = angles->Angle_J6;

		arm.SetAngles(Joint_Position);
	}
}

void JacoArm::VelocityMSG(const jaco_driver::JointVelocityConstPtr& joint_vel)
{
	if (!arm.Stopped())
	{
		joint_velocities.Actuator1 = joint_vel->Velocity_J1;
		joint_velocities.Actuator2 = joint_vel->Velocity_J2;
		joint_velocities.Actuator3 = joint_vel->Velocity_J3;
		joint_velocities.Actuator4 = joint_vel->Velocity_J4;
		joint_velocities.Actuator5 = joint_vel->Velocity_J5;
		joint_velocities.Actuator6 = joint_vel->Velocity_J6;
		last_joint_update = ros::Time().now();

		if (joint_vel_timer_flag == false)
		{
			joint_vel_timer.start();
			joint_vel_timer_flag = true;
		}
	}
}

/*!
 * \brief Handler for "stop" service.
 *
 * Instantly stops the arm and prevents further movement until start service is
 * invoked.
 */
bool JacoArm::StopSRV(jaco_driver::Stop::Request &req, jaco_driver::Stop::Response &res)
{
	arm.Stop();
	res.stop_result = "JACO ARM HAS BEEN STOPPED";
	ROS_DEBUG("JACO ARM STOP REQUEST");

	return true;
}

/*!
 * \brief Handler for "start" service.
 *
 * Re-enables control of the arm after a stop.
 */
bool JacoArm::StartSRV(jaco_driver::Start::Request &req, jaco_driver::Start::Response &res)
{
	arm.Start();
	res.start_result = "JACO ARM CONTROL HAS BEEN ENABLED";
	ROS_DEBUG("JACO ARM START REQUEST");

	return true;
}

void JacoArm::CartesianVelocityMSG(const geometry_msgs::TwistStampedConstPtr& cartesian_vel)
{
	if (!arm.Stopped())
	{
		cartesian_velocities.X = cartesian_vel->twist.linear.x;
		cartesian_velocities.Y = cartesian_vel->twist.linear.y;
		cartesian_velocities.Z = cartesian_vel->twist.linear.z;
		cartesian_velocities.ThetaX = cartesian_vel->twist.angular.x;
		cartesian_velocities.ThetaY = cartesian_vel->twist.angular.y;
		cartesian_velocities.ThetaZ = cartesian_vel->twist.angular.z;

		last_cartesian_update = ros::Time().now();

		if (cartesian_vel_timer_flag == false)
		{
			cartesian_vel_timer.start();
			cartesian_vel_timer_flag = true;
		}
	}
}

void JacoArm::CartesianVelTimer(const ros::TimerEvent&)
{
	arm.SetCartesianVelocities(cartesian_velocities);

	if ((ros::Time().now().toSec() - last_cartesian_update.toSec()) > 1)
	{
		cartesian_vel_timer.stop();
		cartesian_vel_timer_flag = false;
	}
}

void JacoArm::JointVelTimer(const ros::TimerEvent&)
{
	arm.SetVelocities(joint_velocities);

	if ((ros::Time().now().toSec() - last_joint_update.toSec()) > 1)
	{
		joint_vel_timer.stop();
		joint_vel_timer_flag = false;
	}
}

/*!
 * \brief Contains coordinates for an alternate "Home" position
 *
 * GoHome() function must be enabled in the initialization routine for this to
 * work.
 */
void JacoArm::GoHome(void)
{
/*
	AngularInfo joint_home;

	joint_home.Actuator1 = 176.0;
	joint_home.Actuator2 = 111.0;
	joint_home.Actuator3 = 107.0;
	joint_home.Actuator4 = 459.0;
	joint_home.Actuator5 = 102.0;
	joint_home.Actuator6 = 106.0;

	SetAngles(joint_home, 10); //send joints to home position

	API->SetCartesianControl();
*/
}

/*!
 * \brief Publishes the current joint angles.
 *
 * Joint angles are published in both their raw state as obtained from the arm
 * (JointAngles), and transformed & converted to radians (joint_state) as per
 * the Jaco Kinematics PDF.
 *
 * JointState will eventually also publish the velocity and effort for each
 * joint, when this data is made available by the C++ API.  Currenty velocity
 * and effort are reported as being zero (0.0) for all joints.
 */
void JacoArm::BroadCastAngles(void)
{
	// Populate an array of joint names.  arm_0_joint is the base, arm_5_joint is the wrist.
	const char* nameArgs[] = {"arm_0_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint"};
	std::vector<std::string> JointName(nameArgs, nameArgs+6);

	AngularPosition arm_angles;

	jaco_driver::JointAngles current_angles;

	sensor_msgs::JointState joint_state;
	joint_state.name = JointName;

	// Define array sizes for the joint_state topic
	joint_state.position.resize(6);
	joint_state.velocity.resize(6);
	joint_state.effort.resize(6);

	memset(&arm_angles, 0, sizeof(arm_angles)); //zero structure

	//Query arm for current joint angles
	arm.GetAngles(arm_angles.Actuators);

	// Raw joint angles
	current_angles.Angle_J1 = arm_angles.Actuators.Actuator1;
	current_angles.Angle_J2 = arm_angles.Actuators.Actuator2;
	current_angles.Angle_J3 = arm_angles.Actuators.Actuator3;
	current_angles.Angle_J4 = arm_angles.Actuators.Actuator4;
	current_angles.Angle_J5 = arm_angles.Actuators.Actuator5;
	current_angles.Angle_J6 = arm_angles.Actuators.Actuator6;

	// Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
	joint_state.position[0] = (180.0 - arm_angles.Actuators.Actuator1) / (180.0 / PI);
	joint_state.position[1] = (arm_angles.Actuators.Actuator2 - 270.0) / (180.0 / PI);
	joint_state.position[2] = (90.0 - arm_angles.Actuators.Actuator3) / (180.0 / PI);
	joint_state.position[3] = (180.0 - arm_angles.Actuators.Actuator4) / (180.0 / PI);
	joint_state.position[4] = (180.0 - arm_angles.Actuators.Actuator5) / (180.0 / PI);
	joint_state.position[5] = (260.0 - arm_angles.Actuators.Actuator6) / (180.0 / PI);

	//Publish the joint state messages

	JointAngles_pub.publish(current_angles); // Publishes the raw joint angles in a custom message.

	JointState_pub.publish(joint_state);     // Publishes the transformed angles in a standard sensor_msgs format.

}

/*!
 * \brief Publishes the current cartesian coordinates
 */
void JacoArm::BroadCastPosition(void)
{
	JacoPose pose;
	geometry_msgs::PoseStamped current_position;

	arm.GetPosition(pose);
	current_position.pose = pose.Pose();

	ToolPosition_pub.publish(current_position);
}

void JacoArm::BroadCastFingerPosition(void)
{

/*
Publishes the current finger positions.
*/

	CartesianPosition Jaco_Position;
	jaco_driver::FingerPosition finger_position;

	memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure
	arm.GetFingers(Jaco_Position.Fingers);

	finger_position.Finger_1 = Jaco_Position.Fingers.Finger1;
	finger_position.Finger_2 = Jaco_Position.Fingers.Finger2;
	finger_position.Finger_3 = Jaco_Position.Fingers.Finger3;

	FingerPosition_pub.publish(finger_position);
}

void JacoArm::StatusTimer(const ros::TimerEvent&)
{
	BroadCastAngles();
	BroadCastPosition();
	BroadCastFingerPosition();
}

}
<commit_msg>Small bugfix.<commit_after>//============================================================================
// Name        : jaco_arm.cpp
// Author      : WPI, Clearpath Robotics
// Version     : 0.5
// Copyright   : BSD
// Description : A ROS driver for controlling the Kinova Jaco robotic manipulator arm
//============================================================================

#include "jaco_driver/jaco_arm.h"

namespace jaco
{

JacoArm::JacoArm(JacoComm &arm_comm, ros::NodeHandle &nh) : arm(arm_comm)
{
	std::string joint_velocity_topic, joint_angles_topic, cartesian_velocity_topic,
		tool_position_topic, set_finger_position_topic, finger_position_topic, joint_state_topic,
		set_joint_angle_topic;

	nh.param<std::string>("joint_velocity_topic", joint_velocity_topic, "joint_velocity");
	nh.param<std::string>("joint_angles_topic", joint_angles_topic, "joint_angles");
	nh.param<std::string>("cartesian_velocity_topic", cartesian_velocity_topic, "cartesian_velocity");
	nh.param<std::string>("tool_position_topic", tool_position_topic, "tool_position");
	nh.param<std::string>("set_finger_position_topic", set_finger_position_topic, "set_finger_position");
	nh.param<std::string>("finger_position_topic", finger_position_topic, "finger_position");
	nh.param<std::string>("joint_state_topic", joint_state_topic, "joint_state");
	nh.param<std::string>("set_joint_angle_topic", set_joint_angle_topic, "set_joint_angle");

	//Print out received topics
	ROS_DEBUG("Got Joint Velocity Topic Name: <%s>", joint_velocity_topic.c_str());
	ROS_DEBUG("Got Joint Angles Topic Name: <%s>", joint_angles_topic.c_str());
	ROS_DEBUG("Got Cartesian Velocity Topic Name: <%s>", cartesian_velocity_topic.c_str());
	ROS_DEBUG("Got Tool Position Topic Name: <%s>", tool_position_topic.c_str());
	ROS_DEBUG("Got Set Finger Position Topic Name: <%s>", set_finger_position_topic.c_str());
	ROS_DEBUG("Got Finger Position Topic Name: <%s>", finger_position_topic.c_str());
	ROS_DEBUG("Got Joint State Topic Name: <%s>", joint_state_topic.c_str());
	ROS_DEBUG("Got Set Joint Angle Topic Name: <%s>", set_joint_angle_topic.c_str());

	ROS_INFO("Starting Up Jaco Arm Controller...");

	previous_state = 0;

	/* Set up Services */
	stop_service = nh.advertiseService("stop", &JacoArm::StopSRV, this);
	start_service = nh.advertiseService("start", &JacoArm::StartSRV, this);
	homing_service = nh.advertiseService("home_arm", &JacoArm::HomeArmSRV, this);

	/* Set Default Configuration */

	// API->RestoreFactoryDefault(); // uncomment comment ONLY if you want to lose your settings on each launch.

	ClientConfigurations configuration;
	arm.GetConfig(configuration);
	arm.PrintConfig(configuration);

	last_update_time = ros::Time::now();
	update_time = ros::Duration(5.0);

	/* Storing arm in home position */

	/* Set up Publishers */
	JointAngles_pub = nh.advertise<jaco_driver::JointAngles>(joint_angles_topic, 2);
	JointState_pub = nh.advertise<sensor_msgs::JointState>(joint_state_topic, 2);
	ToolPosition_pub = nh.advertise<geometry_msgs::PoseStamped>(tool_position_topic, 2);
	FingerPosition_pub = nh.advertise<jaco_driver::FingerPosition>(finger_position_topic, 2);

	/* Set up Subscribers*/
	JointVelocity_sub = nh.subscribe(joint_velocity_topic, 1, &JacoArm::VelocityMSG, this);
	CartesianVelocity_sub = nh.subscribe(cartesian_velocity_topic, 1, &JacoArm::CartesianVelocityMSG, this);
	SetFingerPosition_sub = nh.subscribe(set_finger_position_topic, 1, &JacoArm::SetFingerPositionMSG, this);
	SetJoint_sub = nh.subscribe(set_joint_angle_topic, 1, &JacoArm::SetJointAnglesMSG, this);

	status_timer = nh.createTimer(ros::Duration(0.05), &JacoArm::StatusTimer, this);

	joint_vel_timer = nh.createTimer(ros::Duration(0.01), &JacoArm::JointVelTimer, this);
	joint_vel_timer.stop();
	joint_vel_timer_flag = false;
	cartesian_vel_timer = nh.createTimer(ros::Duration(0.01), &JacoArm::CartesianVelTimer, this);
	cartesian_vel_timer.stop();
	cartesian_vel_timer_flag = false;

	ROS_INFO("The Arm is ready to use.");

	TrajectoryPoint Jaco_Velocity;

	memset(&Jaco_Velocity, 0, sizeof(Jaco_Velocity)); //zero structure

	arm.SetCartesianVelocities(Jaco_Velocity.Position.CartesianPosition);
}

JacoArm::~JacoArm()
{
}

bool JacoArm::HomeArmSRV(jaco_driver::HomeArm::Request &req, jaco_driver::HomeArm::Response &res)
{
	arm.HomeArm();
	res.homearm_result = "JACO ARM HAS BEEN RETURNED HOME";

	return true;
}

/*!
 * \brief Receives ROS command messages and relays them to SetFingers.
 */
void JacoArm::SetFingerPositionMSG(const jaco_driver::FingerPositionConstPtr& finger_pos)
{
	if (!arm.Stopped())
	{
		FingersPosition Finger_Position;
		memset(&Finger_Position, 0, sizeof(Finger_Position)); //zero structure

		Finger_Position.Finger1 = finger_pos->Finger_1;
		Finger_Position.Finger2 = finger_pos->Finger_2;
		Finger_Position.Finger3 = finger_pos->Finger_3;

		arm.SetFingers(Finger_Position);
	}
}

/*!
 * \brief Receives ROS command messages and relays them to SetAngles.
 */
void JacoArm::SetJointAnglesMSG(const jaco_driver::JointAnglesConstPtr& angles)
{
	if (!arm.Stopped())
	{
		AngularInfo Joint_Position;
		memset(&Joint_Position, 0, sizeof(Joint_Position)); //zero structure

		Joint_Position.Actuator1 = angles->Angle_J1;
		Joint_Position.Actuator2 = angles->Angle_J2;
		Joint_Position.Actuator3 = angles->Angle_J3;
		Joint_Position.Actuator4 = angles->Angle_J4;
		Joint_Position.Actuator5 = angles->Angle_J5;
		Joint_Position.Actuator6 = angles->Angle_J6;

		arm.SetAngles(Joint_Position);
	}
}

void JacoArm::VelocityMSG(const jaco_driver::JointVelocityConstPtr& joint_vel)
{
	if (!arm.Stopped())
	{
		joint_velocities.Actuator1 = joint_vel->Velocity_J1;
		joint_velocities.Actuator2 = joint_vel->Velocity_J2;
		joint_velocities.Actuator3 = joint_vel->Velocity_J3;
		joint_velocities.Actuator4 = joint_vel->Velocity_J4;
		joint_velocities.Actuator5 = joint_vel->Velocity_J5;
		joint_velocities.Actuator6 = joint_vel->Velocity_J6;
		last_joint_update = ros::Time().now();

		if (joint_vel_timer_flag == false)
		{
			joint_vel_timer.start();
			joint_vel_timer_flag = true;
		}
	}
}

/*!
 * \brief Handler for "stop" service.
 *
 * Instantly stops the arm and prevents further movement until start service is
 * invoked.
 */
bool JacoArm::StopSRV(jaco_driver::Stop::Request &req, jaco_driver::Stop::Response &res)
{
	arm.Stop();
	res.stop_result = "JACO ARM HAS BEEN STOPPED";
	ROS_DEBUG("JACO ARM STOP REQUEST");

	return true;
}

/*!
 * \brief Handler for "start" service.
 *
 * Re-enables control of the arm after a stop.
 */
bool JacoArm::StartSRV(jaco_driver::Start::Request &req, jaco_driver::Start::Response &res)
{
	arm.Start();
	res.start_result = "JACO ARM CONTROL HAS BEEN ENABLED";
	ROS_DEBUG("JACO ARM START REQUEST");

	return true;
}

void JacoArm::CartesianVelocityMSG(const geometry_msgs::TwistStampedConstPtr& cartesian_vel)
{
	if (!arm.Stopped())
	{
		cartesian_velocities.X = cartesian_vel->twist.linear.x;
		cartesian_velocities.Y = cartesian_vel->twist.linear.y;
		cartesian_velocities.Z = cartesian_vel->twist.linear.z;
		cartesian_velocities.ThetaX = cartesian_vel->twist.angular.x;
		cartesian_velocities.ThetaY = cartesian_vel->twist.angular.y;
		cartesian_velocities.ThetaZ = cartesian_vel->twist.angular.z;

		last_cartesian_update = ros::Time().now();

		if (cartesian_vel_timer_flag == false)
		{
			cartesian_vel_timer.start();
			cartesian_vel_timer_flag = true;
		}
	}
}

void JacoArm::CartesianVelTimer(const ros::TimerEvent&)
{
	arm.SetCartesianVelocities(cartesian_velocities);

	if ((ros::Time().now().toSec() - last_cartesian_update.toSec()) > 1)
	{
		cartesian_vel_timer.stop();
		cartesian_vel_timer_flag = false;
	}
}

void JacoArm::JointVelTimer(const ros::TimerEvent&)
{
	arm.SetVelocities(joint_velocities);

	if ((ros::Time().now().toSec() - last_joint_update.toSec()) > 1)
	{
		joint_vel_timer.stop();
		joint_vel_timer_flag = false;
	}
}

/*!
 * \brief Contains coordinates for an alternate "Home" position
 *
 * GoHome() function must be enabled in the initialization routine for this to
 * work.
 */
void JacoArm::GoHome(void)
{
/*
	AngularInfo joint_home;

	joint_home.Actuator1 = 176.0;
	joint_home.Actuator2 = 111.0;
	joint_home.Actuator3 = 107.0;
	joint_home.Actuator4 = 459.0;
	joint_home.Actuator5 = 102.0;
	joint_home.Actuator6 = 106.0;

	SetAngles(joint_home, 10); //send joints to home position

	API->SetCartesianControl();
*/
}

/*!
 * \brief Publishes the current joint angles.
 *
 * Joint angles are published in both their raw state as obtained from the arm
 * (JointAngles), and transformed & converted to radians (joint_state) as per
 * the Jaco Kinematics PDF.
 *
 * JointState will eventually also publish the velocity and effort for each
 * joint, when this data is made available by the C++ API.  Currenty velocity
 * and effort are reported as being zero (0.0) for all joints.
 */
void JacoArm::BroadCastAngles(void)
{
	// Populate an array of joint names.  arm_0_joint is the base, arm_5_joint is the wrist.
	const char* nameArgs[] = {"arm_0_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint"};
	std::vector<std::string> JointName(nameArgs, nameArgs+6);

	AngularPosition arm_angles;

	jaco_driver::JointAngles current_angles;

	sensor_msgs::JointState joint_state;
	joint_state.name = JointName;

	// Define array sizes for the joint_state topic
	joint_state.position.resize(6);
	joint_state.velocity.resize(6);
	joint_state.effort.resize(6);

	memset(&arm_angles, 0, sizeof(arm_angles)); //zero structure

	//Query arm for current joint angles
	arm.GetAngles(arm_angles.Actuators);

	// Raw joint angles
	current_angles.Angle_J1 = arm_angles.Actuators.Actuator1;
	current_angles.Angle_J2 = arm_angles.Actuators.Actuator2;
	current_angles.Angle_J3 = arm_angles.Actuators.Actuator3;
	current_angles.Angle_J4 = arm_angles.Actuators.Actuator4;
	current_angles.Angle_J5 = arm_angles.Actuators.Actuator5;
	current_angles.Angle_J6 = arm_angles.Actuators.Actuator6;

	// Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
	joint_state.position[0] = (180.0 - arm_angles.Actuators.Actuator1) / (180.0 / M_PI);
	joint_state.position[1] = (arm_angles.Actuators.Actuator2 - 270.0) / (180.0 / M_PI);
	joint_state.position[2] = (90.0 - arm_angles.Actuators.Actuator3) / (180.0 / M_PI);
	joint_state.position[3] = (180.0 - arm_angles.Actuators.Actuator4) / (180.0 / M_PI);
	joint_state.position[4] = (180.0 - arm_angles.Actuators.Actuator5) / (180.0 / M_PI);
	joint_state.position[5] = (260.0 - arm_angles.Actuators.Actuator6) / (180.0 / M_PI);

	//Publish the joint state messages

	JointAngles_pub.publish(current_angles); // Publishes the raw joint angles in a custom message.

	JointState_pub.publish(joint_state);     // Publishes the transformed angles in a standard sensor_msgs format.

}

/*!
 * \brief Publishes the current cartesian coordinates
 */
void JacoArm::BroadCastPosition(void)
{
	JacoPose pose;
	geometry_msgs::PoseStamped current_position;

	arm.GetPosition(pose);
	current_position.pose = pose.Pose();

	ToolPosition_pub.publish(current_position);
}

void JacoArm::BroadCastFingerPosition(void)
{

/*
Publishes the current finger positions.
*/

	CartesianPosition Jaco_Position;
	jaco_driver::FingerPosition finger_position;

	memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure
	arm.GetFingers(Jaco_Position.Fingers);

	finger_position.Finger_1 = Jaco_Position.Fingers.Finger1;
	finger_position.Finger_2 = Jaco_Position.Fingers.Finger2;
	finger_position.Finger_3 = Jaco_Position.Fingers.Finger3;

	FingerPosition_pub.publish(finger_position);
}

void JacoArm::StatusTimer(const ros::TimerEvent&)
{
	BroadCastAngles();
	BroadCastPosition();
	BroadCastFingerPosition();
}

}
<|endoftext|>


<commit_before>/**
 * @file /sophus_ros_conversions/include/sophus_ros_conversions/geometry.hpp
 */
/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef sophus_ros_conversions_GEOMETRY_HPP_
#define sophus_ros_conversions_GEOMETRY_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Vector3.h>
#include <sophus/se3.hpp>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace sophus_ros_conversions {

/*****************************************************************************
** Interfaces
*****************************************************************************/

/**
 * Converts a ros pose message to a sophus pose.
 *
 * @param p
 * @param s
 */
void poseMsgToSophus(const geometry_msgs::Pose &p, Sophus::SE3f &s);

/**
 * Converts a sophus pose to a ros pose message.
 * @param s
 * @return
 */
geometry_msgs::Pose sophusToPoseMsg(const Sophus::SE3f& s);

/**
 * Converts vector3f, usually used as part of ros transform messages into
 * the translation part that gets used in Sophus::SE3f types. Note that
 * sophus uses SE3f::Point as the translation type in the constructors of SE3f types.
 */
void vector3MsgToSophus(const geometry_msgs::Vector3 &v, Sophus::SE3f::Point &translation);

/**
 * Converts ros transform message to a homogenous transform in sophus.
 * @param transform
 * @param se3
 */
void transformMsgToSophus(const geometry_msgs::Transform &transform, Sophus::SE3f &se3);

/**
 * Alternate form of ros transform -> sophus conversion.
 *
 * @param transform
 * @return Sophus::SE3f
 */
Sophus::SE3f transformMsgToSophus(const geometry_msgs::Transform &transform);

/**
 * Convert sophus transform to ros transform.
 * @param se3
 * @return geometry_msgs::Transform
 */
geometry_msgs::Transform sophusToTransformMsg(const Sophus::SE3f& se3);

/*****************************************************************************
** Trailers
*****************************************************************************/

} // namespace sophus_ros_conversions

#endif /* sophus_ros_conversions_GEOMETRY_HPP_ */
<commit_msg>add function to convert tf::stampedTransform to sophus<commit_after>/**
 * @file /sophus_ros_conversions/include/sophus_ros_conversions/geometry.hpp
 */
/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef sophus_ros_conversions_GEOMETRY_HPP_
#define sophus_ros_conversions_GEOMETRY_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Vector3.h>
#include <sophus/se3.hpp>
#include <tf/transform_listener.h>
/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace sophus_ros_conversions {

/*****************************************************************************
** Interfaces
*****************************************************************************/

/**
 * Converts a ros pose message to a sophus pose.
 *
 * @param p
 * @param s
 */
void poseMsgToSophus(const geometry_msgs::Pose &p, Sophus::SE3f &s);

/**
 * Converts a sophus pose to a ros pose message.
 * @param s
 * @return
 */
geometry_msgs::Pose sophusToPoseMsg(const Sophus::SE3f& s);

/**
 * Converts vector3f, usually used as part of ros transform messages into
 * the translation part that gets used in Sophus::SE3f types. Note that
 * sophus uses SE3f::Point as the translation type in the constructors of SE3f types.
 */
void vector3MsgToSophus(const geometry_msgs::Vector3 &v, Sophus::SE3f::Point &translation);

/**
 * Converts ros transform message to a homogenous transform in sophus.
 * @param transform
 * @param se3
 */
void transformMsgToSophus(const geometry_msgs::Transform &transform, Sophus::SE3f &se3);

template<typename T>
void stampedTransformToSophus( const tf::StampedTransform & transform, Sophus::SE3Group<T> & se3 )
{
	Eigen::Quaternion<T> q;
	Eigen::Matrix<T,3,1> t;

	q.x() = transform.getRotation().getX();
	q.y() = transform.getRotation().getY();
	q.z() = transform.getRotation().getZ();
	q.w() = transform.getRotation().getW();
	t.x() = transform.getOrigin().getX();
	t.y() = transform.getOrigin().getY();
	t.z() = transform.getOrigin().getZ();
	se3 = Sophus::SE3Group<T>(q,t);
}

/**
 * Alternate form of ros transform -> sophus conversion.
 *
 * @param transform
 * @return Sophus::SE3f
 */
Sophus::SE3f transformMsgToSophus(const geometry_msgs::Transform &transform);

/**
 * Convert sophus transform to ros transform.
 * @param se3
 * @return geometry_msgs::Transform
 */
geometry_msgs::Transform sophusToTransformMsg(const Sophus::SE3f& se3);

/*****************************************************************************
** Trailers
*****************************************************************************/

} // namespace sophus_ros_conversions

#endif /* sophus_ros_conversions_GEOMETRY_HPP_ */
<|endoftext|>


<commit_before>/* GNE - Game Networking Engine, a portable multithreaded networking library.
 * Copyright (C) 2001 Jason Winnebeck (gillius@webzone.net)
 * Project website: http://www.rit.edu/~jpw9607/
 *
 * This library 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 2.1 of the License, or (at your option) any later version.
 *
 * This library 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 this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "gneintern.h"
#include "ListServerConnection.h"

//##ModelId=3AE4AA420046
ListServerConnection::ListServerConnection(std::string address)
: ClientConnection(0, 0, address) {
}

//##ModelId=3AE4AA420078
ListServerConnection::~ListServerConnection() {
}

//##ModelId=3AE4AAE000C8
void ListServerConnection::setGame(std::string gameName) {
}

//##ModelId=3AE4AB3A0000
void ListServerConnection::getGameList() {
}

<commit_msg>ctor change.<commit_after>/* GNE - Game Networking Engine, a portable multithreaded networking library.
 * Copyright (C) 2001 Jason Winnebeck (gillius@webzone.net)
 * Project website: http://www.rit.edu/~jpw9607/
 *
 * This library 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 2.1 of the License, or (at your option) any later version.
 *
 * This library 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 this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "gneintern.h"
#include "ListServerConnection.h"

//##ModelId=3AE4AA420046
ListServerConnection::ListServerConnection(std::string address)
: ClientConnection(0, 0) {
}

//##ModelId=3AE4AA420078
ListServerConnection::~ListServerConnection() {
}

//##ModelId=3AE4AAE000C8
void ListServerConnection::setGame(std::string gameName) {
}

//##ModelId=3AE4AB3A0000
void ListServerConnection::getGameList() {
}

<|endoftext|>


<commit_before>#include "ArraySort.h"
namespace NP_ARRAYSORT
{
	void quickSort(Comparable ** array, int fromIndex, int toIndex)
	{
		if (fromIndex < toIndex) {
			int position = partition(array, fromIndex, toIndex);
			quickSort(array, fromIndex, position - 1);
			quickSort(array, position + 1, toIndex);
		}
	}

	int partition(Comparable ** array, int fromIndex, int toIndex)
	{
		Comparable * pivot = array[toIndex];
		int small = fromIndex - 1;
		for (int k = fromIndex; k < toIndex; k++) {
			if (*array[k] <= *pivot) {
				small++;
				swap(array, k, small);
			}
		}
		swap(array, toIndex, small + 1);
		return small + 1;
	}

	//-----------------------------------------------------------------------------
	//    function:		SortFirstMiddleLast
	//    description:	Sort the three items
	//
	//    Called By:	all sorting algs
	//
	//    Parameters:	Comparable ** array, int loIndex, int midIndex, int hiIndex
	//
	//    Returns:          void
	//
	//    History Log:
	//			4/27/17  NP  completed version 1.0
	// ----------------------------------------------------------------------------
	void SortFirstMiddleLast(Comparable ** array, int loIndex, int midIndex, int hiIndex)
	{
		Comparable * low = array[loIndex];
		Comparable * mid = array[midIndex];
		Comparable * hi = array[hiIndex];
		while (!((low < mid) && (mid < hi))) {
			if (low > mid)
				swap(array, loIndex, midIndex);
			if (mid > hi)
				swap(array, midIndex, hiIndex);
			if (low > hi)
				swap(array, loIndex, hiIndex);
			low = array[loIndex];
			mid = array[midIndex];
			hi = array[hiIndex];
		}
	}

	//-----------------------------------------------------------------------------
	//    function:		swap
	//    description:	swaps two items in an array
	//
	//    Input:		Comparable ** array, int index1, int index2
	//
	//    Called By:	all sorting algs
	//
	//    Parameters:	Comparable ** array, int index1, int index2
	//
	//    Returns:          void
	//
	//    History Log:
	//			4/27/17  NP  completed version 1.0
	// ----------------------------------------------------------------------------
	void swap(Comparable ** array, int index1, int index2) {
		Comparable * tmp = array[index1];
		array[index1] = array[index2];
		array[index2] = tmp;
	}

	void insertionSort(Comparable ** array, int fromIndex, int toIndex)
	{
		if (fromIndex < 1)
			fromIndex = 1;

		for (int outerIndex = fromIndex; outerIndex < toIndex; outerIndex++) {
			int innerIndex = outerIndex - 1;
			Comparable * temp = array[outerIndex];

			while (innerIndex >= 0 && *temp < *array[innerIndex]) {
				array[innerIndex + 1] = array[innerIndex];
				innerIndex--;
			}

			array[innerIndex + 1] = temp;
		}
	}

	//-----------------------------------------------------------------------------
	//    function:		safeRead(istream& sin, Comparable* d, const char* re)
	//    description:	safely reads in a DateTime (d) from sin.
	//				Re-prompts and re-enters if input is invalid
	//
	//    Input:		Comparable* d from sin
	//
	//    Called By:	main
	//
	//    Parameters:	istream& sin  -- the input stream
	//			Comparable* d -- pointer to the object input
	//			const char* prompt  -- prompt, if necessary
	//
	//    Returns:          void
	//
	//    History Log:
	//			2/9/08  PB  completed version 1.0
	// ----------------------------------------------------------------------------
	void safeRead(istream& sin, Comparable* d, const char* prompt)
	{
		const int BUFFERSIZE = 256;
		d->input(sin);
		while (!sin)
		{	// read in number--enter loop if fail
			sin.clear();		// clear fail
			sin.ignore(BUFFERSIZE, '\n');	// read past newline
			cout << prompt;  // re-prompt
			d->input(sin);
		}
		// read past newline once input succeeds
		sin.ignore(BUFFERSIZE, '\n');
	}

	//-----------------------------------------------------------------------------
	///    function:	void printArray(ostream & sout, Comparable **a, int size)
	///    description:	can print out an array of DateTime *
	//
	//    Output:		Comparable* d sout
	//
	//    Called By:	main
	//
	//    Parameters:	ostream& sout  -- the output stream
	//			Comparable** a -- array of pointers to the objects
	//			int size  -- number of elements in the array
	//
	//    Returns:          void
	//
	//    History Log:
	//			2/9/08  PB  completed version 1.0
	// ----------------------------------------------------------------------------
	void printArray(ostream & sout, Comparable **array, int len)
	{
		for (int i = 0; i < len; i++)
		{
			array[i]->print(sout);
			if (i + 1 < len)
				sout << ", ";
		}

		sout << endl;
	}
}

<commit_msg>use "optimized" sort<commit_after>#include "ArraySort.h"
namespace NP_ARRAYSORT
{
	void quickSort(Comparable ** array, int fromIndex, int toIndex)
	{
		if (fromIndex + 4 < toIndex) {
			int position = partition(array, fromIndex, toIndex);
			quickSort(array, fromIndex, position - 1);
			quickSort(array, position + 1, toIndex);
		} else if (fromIndex < toIndex) {
			insertionSort(array, fromIndex, toIndex);
		}
	}

	int partition(Comparable ** array, int fromIndex, int toIndex)
	{
		Comparable * pivot = array[toIndex];
		int small = fromIndex - 1;
		for (int index = fromIndex; index < toIndex; index++) {
			if (*array[index] <= *pivot) {
				small++;
				swap(array, index, small);
			}
		}
		swap(array, toIndex, small + 1);
		return small + 1;
	}

	//-----------------------------------------------------------------------------
	//    function:		SortFirstMiddleLast
	//    description:	Sort the three items
	//
	//    Called By:	all sorting algs
	//
	//    Parameters:	Comparable ** array, int loIndex, int midIndex, int hiIndex
	//
	//    Returns:          void
	//
	//    History Log:
	//			4/27/17  NP  completed version 1.0
	// ----------------------------------------------------------------------------
	void SortFirstMiddleLast(Comparable ** array, int loIndex, int midIndex, int hiIndex)
	{
		Comparable * low = array[loIndex];
		Comparable * mid = array[midIndex];
		Comparable * hi = array[hiIndex];
		while ((low > mid) || (mid > hi)) {
			if (low > mid)
				swap(array, loIndex, midIndex);
			if (mid > hi)
				swap(array, midIndex, hiIndex);
			if (low > hi)
				swap(array, loIndex, hiIndex);
			low = array[loIndex];
			mid = array[midIndex];
			hi = array[hiIndex];
		}
	}

	//-----------------------------------------------------------------------------
	//    function:		swap
	//    description:	swaps two items in an array
	//
	//    Input:		Comparable ** array, int index1, int index2
	//
	//    Called By:	all sorting algs
	//
	//    Parameters:	Comparable ** array, int index1, int index2
	//
	//    Returns:          void
	//
	//    History Log:
	//			4/27/17  NP  completed version 1.0
	// ----------------------------------------------------------------------------
	void swap(Comparable ** array, int index1, int index2) {
		Comparable * tmp = array[index1];
		array[index1] = array[index2];
		array[index2] = tmp;
	}

	void insertionSort(Comparable ** array, int fromIndex, int toIndex)
	{
		if (fromIndex < 1)
			fromIndex = 1;

		for (int outerIndex = fromIndex; outerIndex < toIndex; outerIndex++) {
			int innerIndex = outerIndex - 1;
			Comparable * temp = array[outerIndex];

			while (innerIndex >= 0 && *temp < *array[innerIndex]) {
				array[innerIndex + 1] = array[innerIndex];
				innerIndex--;
			}

			array[innerIndex + 1] = temp;
		}
	}

	//-----------------------------------------------------------------------------
	//    function:		safeRead(istream& sin, Comparable* d, const char* re)
	//    description:	safely reads in a DateTime (d) from sin.
	//				Re-prompts and re-enters if input is invalid
	//
	//    Input:		Comparable* d from sin
	//
	//    Called By:	main
	//
	//    Parameters:	istream& sin  -- the input stream
	//			Comparable* d -- pointer to the object input
	//			const char* prompt  -- prompt, if necessary
	//
	//    Returns:          void
	//
	//    History Log:
	//			2/9/08  PB  completed version 1.0
	// ----------------------------------------------------------------------------
	void safeRead(istream& sin, Comparable* d, const char* prompt)
	{
		const int BUFFERSIZE = 256;
		d->input(sin);
		while (!sin)
		{	// read in number--enter loop if fail
			sin.clear();		// clear fail
			sin.ignore(BUFFERSIZE, '\n');	// read past newline
			cout << prompt;  // re-prompt
			d->input(sin);
		}
		// read past newline once input succeeds
		sin.ignore(BUFFERSIZE, '\n');
	}

	//-----------------------------------------------------------------------------
	///    function:	void printArray(ostream & sout, Comparable **a, int size)
	///    description:	can print out an array of DateTime *
	//
	//    Output:		Comparable* d sout
	//
	//    Called By:	main
	//
	//    Parameters:	ostream& sout  -- the output stream
	//			Comparable** a -- array of pointers to the objects
	//			int size  -- number of elements in the array
	//
	//    Returns:          void
	//
	//    History Log:
	//			2/9/08  PB  completed version 1.0
	// ----------------------------------------------------------------------------
	void printArray(ostream & sout, Comparable **array, int len)
	{
		for (int i = 0; i < len; i++)
		{
			array[i]->print(sout);
			if (i + 1 < len)
				sout << ", ";
		}

		sout << endl;
	}
}

<|endoftext|>


<commit_before>/// \file ROOT/TCanvas.hxx
/// \ingroup Gpad ROOT7
/// \author Axel Naumann <axel@cern.ch>
/// \date 2015-07-08
/// \warning This is part of the ROOT 7 prototype! It will change without notice. It might trigger earthquakes. Feedback
/// is welcome!

/*************************************************************************
 * Copyright (C) 1995-2015, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

#ifndef ROOT7_TCanvas
#define ROOT7_TCanvas

#include "ROOT/TColor.hxx"
#include "ROOT/TPad.hxx"
#include "ROOT/TVirtualCanvasPainter.hxx"

#include <memory>
#include <string>
#include <vector>

namespace ROOT {
namespace Experimental {

class TDrawingOptsBaseNoDefault;
template <class PRIMITIVE>
class TDrawingAttrRef;

/** \class ROOT::Experimental::TCanvas
  A window's topmost `TPad`.
  Access is through TCanvasPtr.
  */

class TCanvas: public TPadBase {
private:
   /// Title of the canvas.
   std::string fTitle;

   /// Size of the canvas in pixels,
   std::array<TPadCoord::Pixel, 2> fSize;

   /// Colors used by drawing options in the pad and any sub-pad.
   Internal::TDrawingAttrTable<TColor> fColorTable;

   /// Integers used by drawing options in the pad and any sub-pad.
   Internal::TDrawingAttrTable<long long> fIntAttrTable;

   /// Floating points used by drawing options in the pad and any sub-pad.
   Internal::TDrawingAttrTable<double> fFPAttrTable;

   /// Modify counter, incremented every time canvas is changed
   uint64_t fModified; ///<!

   /// The painter of this canvas, bootstrapping the graphics connection.
   /// Unmapped canvases (those that never had `Draw()` invoked) might not have
   /// a painter.
   std::unique_ptr<Internal::TVirtualCanvasPainter> fPainter; ///<!

   /// Disable copy construction for now.
   TCanvas(const TCanvas &) = delete;

   /// Disable assignment for now.
   TCanvas &operator=(const TCanvas &) = delete;

   ///\{
   ///\name Drawing options attribute handling

   /// Attribute table (non-const access).
   Internal::TDrawingAttrTable<TColor> &GetAttrTable(TColor *) { return fColorTable; }
   Internal::TDrawingAttrTable<long long> &GetAttrTable(long long *) { return fIntAttrTable; }
   Internal::TDrawingAttrTable<double> &GetAttrTable(double *) { return fFPAttrTable; }

   /// Attribute table (const access).
   const Internal::TDrawingAttrTable<TColor> &GetAttrTable(TColor *) const { return fColorTable; }
   const Internal::TDrawingAttrTable<long long> &GetAttrTable(long long *) const { return fIntAttrTable; }
   const Internal::TDrawingAttrTable<double> &GetAttrTable(double *) const { return fFPAttrTable; }

   friend class ROOT::Experimental::TDrawingOptsBaseNoDefault;
   template <class PRIMITIVE>
   friend class ROOT::Experimental::TDrawingAttrRef;
   ///\}

public:
   static std::shared_ptr<TCanvas> Create(const std::string &title);

   /// Create a temporary TCanvas; for long-lived ones please use Create().
   TCanvas() = default;

   ~TCanvas() { Wipe(); /* FIXME: this should become Attrs owned and referenced by the TPads */}

   const TCanvas &GetCanvas() const override { return *this; }

   /// Access to the top-most canvas, if any (non-const version).
   TCanvas &GetCanvas() override { return *this; }

   /// Return canvas pixel size as array with two elements - width and height
   const std::array<TPadCoord::Pixel, 2> &GetSize() const { return fSize; }

   /// Set canvas pixel size as array with two elements - width and height
   void SetSize(const std::array<TPadCoord::Pixel, 2> &sz) { fSize = sz; }

   /// Set canvas pixel size - width and height
   void SetSize(const TPadCoord::Pixel &width, const TPadCoord::Pixel &height)
   {
      fSize[0] = width;
      fSize[1] = height;
   }

   /// Display the canvas.
   void Show(const std::string &where = "");

   /// Close all canvas displays
   void Hide();

   /// Insert panel into the canvas, canvas should be shown at this moment
   template <class PANEL>
   bool AddPanel(std::shared_ptr<PANEL> &panel) {
      if (!fPainter) return false;
      return fPainter->AddPanel(panel->GetWindow());
   }

   // Indicates that primitives list was changed or any primitive was modified
   void Modified() { fModified++; }

   // Return if canvas was modified and not yet updated
   bool IsModified() const;

   /// update drawing
   void Update(bool async = false, CanvasCallback_t callback = nullptr);

   /// Save canvas in image file
   void SaveAs(const std::string &filename, bool async = false, CanvasCallback_t callback = nullptr);

   /// Get the canvas's title.
   const std::string &GetTitle() const { return fTitle; }

   /// Set the canvas's title.
   void SetTitle(const std::string &title) { fTitle = title; }

   /// Convert a `Pixel` position to Canvas-normalized positions.
   std::array<TPadCoord::Normal, 2> PixelsToNormal(const std::array<TPadCoord::Pixel, 2> &pos) const final
   {
      return {{pos[0] / fSize[0], pos[1] / fSize[1]}};
   }

   static const std::vector<std::shared_ptr<TCanvas>> &GetCanvases();
};

} // namespace Experimental
} // namespace ROOT

#endif
<commit_msg>Remove attr table from canvas.<commit_after>/// \file ROOT/TCanvas.hxx
/// \ingroup Gpad ROOT7
/// \author Axel Naumann <axel@cern.ch>
/// \date 2015-07-08
/// \warning This is part of the ROOT 7 prototype! It will change without notice. It might trigger earthquakes. Feedback
/// is welcome!

/*************************************************************************
 * Copyright (C) 1995-2015, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

#ifndef ROOT7_TCanvas
#define ROOT7_TCanvas

#include "ROOT/TPad.hxx"
#include "ROOT/TVirtualCanvasPainter.hxx"

#include <memory>
#include <string>
#include <vector>

namespace ROOT {
namespace Experimental {

class TDrawingOptsBaseNoDefault;
template <class PRIMITIVE>
class TDrawingAttrRef;

/** \class ROOT::Experimental::TCanvas
  A window's topmost `TPad`.
  */

class TCanvas: public TPadBase {
private:
   /// Title of the canvas.
   std::string fTitle;

   /// Size of the canvas in pixels,
   std::array<TPadCoord::Pixel, 2> fSize;

   /// Modify counter, incremented every time canvas is changed
   uint64_t fModified; ///<!

   /// The painter of this canvas, bootstrapping the graphics connection.
   /// Unmapped canvases (those that never had `Draw()` invoked) might not have
   /// a painter.
   std::unique_ptr<Internal::TVirtualCanvasPainter> fPainter; ///<!

   /// Disable copy construction for now.
   TCanvas(const TCanvas &) = delete;

   /// Disable assignment for now.
   TCanvas &operator=(const TCanvas &) = delete;

public:
   static std::shared_ptr<TCanvas> Create(const std::string &title);

   /// Create a temporary TCanvas; for long-lived ones please use Create().
   TCanvas() = default;

   ~TCanvas() = default;

   const TCanvas &GetCanvas() const override { return *this; }

   /// Access to the top-most canvas, if any (non-const version).
   TCanvas &GetCanvas() override { return *this; }

   /// Return canvas pixel size as array with two elements - width and height
   const std::array<TPadCoord::Pixel, 2> &GetSize() const { return fSize; }

   /// Set canvas pixel size as array with two elements - width and height
   void SetSize(const std::array<TPadCoord::Pixel, 2> &sz) { fSize = sz; }

   /// Set canvas pixel size - width and height
   void SetSize(const TPadCoord::Pixel &width, const TPadCoord::Pixel &height)
   {
      fSize[0] = width;
      fSize[1] = height;
   }

   /// Display the canvas.
   void Show(const std::string &where = "");

   /// Close all canvas displays
   void Hide();

   /// Insert panel into the canvas, canvas should be shown at this moment
   template <class PANEL>
   bool AddPanel(std::shared_ptr<PANEL> &panel) {
      if (!fPainter) return false;
      return fPainter->AddPanel(panel->GetWindow());
   }

   // Indicates that primitives list was changed or any primitive was modified
   void Modified() { fModified++; }

   // Return if canvas was modified and not yet updated
   bool IsModified() const;

   /// update drawing
   void Update(bool async = false, CanvasCallback_t callback = nullptr);

   /// Save canvas in image file
   void SaveAs(const std::string &filename, bool async = false, CanvasCallback_t callback = nullptr);

   /// Get the canvas's title.
   const std::string &GetTitle() const { return fTitle; }

   /// Set the canvas's title.
   void SetTitle(const std::string &title) { fTitle = title; }

   /// Convert a `Pixel` position to Canvas-normalized positions.
   std::array<TPadCoord::Normal, 2> PixelsToNormal(const std::array<TPadCoord::Pixel, 2> &pos) const final
   {
      return {{pos[0] / fSize[0], pos[1] / fSize[1]}};
   }

   static const std::vector<std::shared_ptr<TCanvas>> &GetCanvases();
};

} // namespace Experimental
} // namespace ROOT

#endif
<|endoftext|>


<commit_before>/*
The OpenTRV project licenses this file to you
under the Apache Licence, Version 2.0 (the "Licence");
you may not use this file except in compliance
with the Licence. You may obtain a copy of the Licence at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing,
software distributed under the Licence is distributed on an
"AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
KIND, either express or implied. See the Licence for the
specific language governing permissions and limitations
under the Licence.

Author(s) / Copyright (s): Damon Hart-Davis 2016
*/

/*
 * OTRadValve CurrentSenseValveMotorDirect tests.
 */

#include <gtest/gtest.h>
#include <cstdint>
#include <cstdio>

#include <OTRadValve.h>

// Test calibration calculations in CurrentSenseValveMotorDirect for REV7/DORM1/TRV1 board.
// Also check some of the use of those calculations.
// In particular test the logic in ModelledRadValveState for starting from extreme positions.
//
// Adapted 2016/10/18 from test_VALVEMODEL.ino testCSVMDC().
TEST(CurrentSenseValveMotorDirect,REV7CSVMDC)
{
    // Compute minimum dead-reckoning ticks as for V0p2/REV7 20161018.
    const uint8_t subcycleTicksRoundedDown_ms = 7;
    const uint8_t minTicks = OTRadValve::CurrentSenseValveMotorDirect::computeMinMotorDRTicks(subcycleTicksRoundedDown_ms);
    ASSERT_EQ(35, minTicks);
    // Compute maximum sub-cycle time to start valve movement as for V0p2/REV7 20161018.
    const uint8_t gst_max = 255;
    const uint8_t minimumMotorRunupTicks = 4; // OTRadValve::ValveMotorDirectV1HardwareDriverBase::minMotorRunupTicks as at 20161018.
    const uint8_t sctAbsLimit = OTRadValve::CurrentSenseValveMotorDirect::computeSctAbsLimit(subcycleTicksRoundedDown_ms, gst_max, minimumMotorRunupTicks);
    ASSERT_EQ(230, sctAbsLimit);

    // Create calibration parameters with values that happen to work for V0p2/REV7.
    OTRadValve::CurrentSenseValveMotorDirect::CalibrationParameters cp(minTicks);
    // Test the calculations with one plausible calibration data set.
    ASSERT_TRUE(cp.updateAndCompute(1601U, 1105U)); // Must not fail...
    ASSERT_EQ(4, cp.getApproxPrecisionPC());
    ASSERT_EQ(25, cp.getTfotcSmall());
    ASSERT_EQ(17, cp.getTfctoSmall());
    // Check that a calibration instance can be reused correctly.
    const uint16_t tfo2 = 1803U;
    const uint16_t tfc2 = 1373U;
    ASSERT_TRUE(cp.updateAndCompute(tfo2, tfc2)); // Must not fail...
    ASSERT_EQ(3, cp.getApproxPrecisionPC());
    ASSERT_EQ(28, cp.getTfotcSmall());
    ASSERT_EQ(21, cp.getTfctoSmall());

    // Check that computing position works...
    // Simple case: fully closed, no accumulated reverse ticks.
    volatile uint16_t ticksFromOpen, ticksReverse;
    ticksFromOpen = tfo2;
    ticksReverse = 0;
    ASSERT_EQ(0, cp.computePosition(ticksFromOpen, ticksReverse));
    ASSERT_EQ(tfo2, ticksFromOpen);
    ASSERT_EQ(0, ticksReverse);
    // Simple case: fully open, no accumulated reverse ticks.
    ticksFromOpen = 0;
    ticksReverse = 0;
    ASSERT_EQ(100, cp.computePosition(ticksFromOpen, ticksReverse));
    ASSERT_EQ(0, ticksFromOpen);
    ASSERT_EQ(0, ticksReverse);
    // Try at half-way mark, no reverse ticks.
    ticksFromOpen = tfo2 / 2;
    ticksReverse = 0;
    ASSERT_EQ(50, cp.computePosition(ticksFromOpen, ticksReverse));
    ASSERT_EQ(tfo2/2, ticksFromOpen);
    ASSERT_EQ(0, ticksReverse);
    // Try at half-way mark with just one reverse tick (nothing should change).
    ticksFromOpen = tfo2 / 2;
    ticksReverse = 1;
    ASSERT_EQ(50, cp.computePosition(ticksFromOpen, ticksReverse));
    ASSERT_EQ(tfo2/2, ticksFromOpen);
    ASSERT_EQ(1, ticksReverse);
    // Try at half-way mark with a big-enough block of reverse ticks to be significant.
    ticksFromOpen = tfo2 / 2;
    ticksReverse = cp.getTfctoSmall();
    ASSERT_EQ(51, cp.computePosition(ticksFromOpen, ticksReverse));
    ASSERT_EQ(tfo2/2 - cp.getTfotcSmall(), ticksFromOpen);
    ASSERT_EQ(0, ticksReverse);
  // DHD20151025: one set of actual measurements during calibration.
  //    ticksFromOpenToClosed: 1529
  //    ticksFromClosedToOpen: 1295
}

class DummyHardwareDriver : public OTRadValve::HardwareMotorDriverInterface
  {
  public:
    // Detect if end-stop is reached or motor current otherwise very high.
    virtual bool isCurrentHigh(OTRadValve::HardwareMotorDriverInterface::motor_drive mdir = motorDriveOpening) const override { return(currentHigh); }
    // isCurrentHigh() returns this value.
    bool currentHigh;
    DummyHardwareDriver() : currentHigh(false) { }
    virtual void motorRun(uint8_t maxRunTicks, motor_drive dir, OTRadValve::HardwareMotorDriverInterfaceCallbackHandler &callback) override
      { }
  };

// Always claims to be at the start of a major cycle.
static uint8_t dummyGetSubCycleTime() { return(0); }

// Test that direct abstract motor drive logic is constructable and minimally sane..
//
// Adapted 2016/10/18 from test_VALVEMODEL.ino testCurrentSenseValveMotorDirect().
TEST(CurrentSenseValveMotorDirect,bascis)
{
    const uint8_t subcycleTicksRoundedDown_ms = 7; // For REV7: OTV0P2BASE::SUBCYCLE_TICK_MS_RD.
    const uint8_t gsct_max = 255; // For REV7: OTV0P2BASE::GSCT_MAX.
    const uint8_t minimumMotorRunupTicks = 4; // For REV7: OTRadValve::ValveMotorDirectV1HardwareDriverBase::minMotorRunupTicks.
    DummyHardwareDriver dhw;
    OTRadValve::CurrentSenseValveMotorDirect csvmd1(&dhw, dummyGetSubCycleTime,
        OTRadValve::CurrentSenseValveMotorDirect::computeMinMotorDRTicks(subcycleTicksRoundedDown_ms),
        OTRadValve::CurrentSenseValveMotorDirect::computeSctAbsLimit(subcycleTicksRoundedDown_ms,
                                                                     gsct_max,
                                                                     minimumMotorRunupTicks));

    // POWER UP
    // Whitebox test of internal state: should be init.
    ASSERT_EQ(OTRadValve::CurrentSenseValveMotorDirect::init, csvmd1.getState());
    // Verify NOT marked as in normal run state immediately upon initialisation.
    ASSERT_TRUE(!csvmd1.isInNormalRunState());
    // Verify NOT marked as in error state immediately upon initialisation.
    ASSERT_TRUE(!csvmd1.isInErrorState());
    // Target % open must start off in a sensible state; fully-closed is good.
    ASSERT_EQ(0, csvmd1.getTargetPC());
    // Until calibration has been successfully run, this should be in non-proportional mode.
    ASSERT_TRUE(csvmd1.inNonProprtionalMode());
    // Nothing passed in requires deferral of (re)calibration.
    ASSERT_FALSE(csvmd1.shouldDeferCalibration());
}

<commit_msg>TODO-1037: tests of shouldDeferCalibration()<commit_after>/*
The OpenTRV project licenses this file to you
under the Apache Licence, Version 2.0 (the "Licence");
you may not use this file except in compliance
with the Licence. You may obtain a copy of the Licence at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing,
software distributed under the Licence is distributed on an
"AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
KIND, either express or implied. See the Licence for the
specific language governing permissions and limitations
under the Licence.

Author(s) / Copyright (s): Damon Hart-Davis 2016
*/

/*
 * OTRadValve CurrentSenseValveMotorDirect tests.
 */

#include <gtest/gtest.h>
#include <cstdint>
#include <cstdio>

#include <OTRadValve.h>

// Test calibration calculations in CurrentSenseValveMotorDirect for REV7/DORM1/TRV1 board.
// Also check some of the use of those calculations.
// In particular test the logic in ModelledRadValveState for starting from extreme positions.
//
// Adapted 2016/10/18 from test_VALVEMODEL.ino testCSVMDC().
TEST(CurrentSenseValveMotorDirect,REV7CSVMDC)
{
    // Compute minimum dead-reckoning ticks as for V0p2/REV7 20161018.
    const uint8_t subcycleTicksRoundedDown_ms = 7;
    const uint8_t minTicks = OTRadValve::CurrentSenseValveMotorDirect::computeMinMotorDRTicks(subcycleTicksRoundedDown_ms);
    ASSERT_EQ(35, minTicks);
    // Compute maximum sub-cycle time to start valve movement as for V0p2/REV7 20161018.
    const uint8_t gst_max = 255;
    const uint8_t minimumMotorRunupTicks = 4; // OTRadValve::ValveMotorDirectV1HardwareDriverBase::minMotorRunupTicks as at 20161018.
    const uint8_t sctAbsLimit = OTRadValve::CurrentSenseValveMotorDirect::computeSctAbsLimit(subcycleTicksRoundedDown_ms, gst_max, minimumMotorRunupTicks);
    ASSERT_EQ(230, sctAbsLimit);

    // Create calibration parameters with values that happen to work for V0p2/REV7.
    OTRadValve::CurrentSenseValveMotorDirect::CalibrationParameters cp(minTicks);
    // Test the calculations with one plausible calibration data set.
    ASSERT_TRUE(cp.updateAndCompute(1601U, 1105U)); // Must not fail...
    ASSERT_EQ(4, cp.getApproxPrecisionPC());
    ASSERT_EQ(25, cp.getTfotcSmall());
    ASSERT_EQ(17, cp.getTfctoSmall());
    // Check that a calibration instance can be reused correctly.
    const uint16_t tfo2 = 1803U;
    const uint16_t tfc2 = 1373U;
    ASSERT_TRUE(cp.updateAndCompute(tfo2, tfc2)); // Must not fail...
    ASSERT_EQ(3, cp.getApproxPrecisionPC());
    ASSERT_EQ(28, cp.getTfotcSmall());
    ASSERT_EQ(21, cp.getTfctoSmall());

    // Check that computing position works...
    // Simple case: fully closed, no accumulated reverse ticks.
    volatile uint16_t ticksFromOpen, ticksReverse;
    ticksFromOpen = tfo2;
    ticksReverse = 0;
    ASSERT_EQ(0, cp.computePosition(ticksFromOpen, ticksReverse));
    ASSERT_EQ(tfo2, ticksFromOpen);
    ASSERT_EQ(0, ticksReverse);
    // Simple case: fully open, no accumulated reverse ticks.
    ticksFromOpen = 0;
    ticksReverse = 0;
    ASSERT_EQ(100, cp.computePosition(ticksFromOpen, ticksReverse));
    ASSERT_EQ(0, ticksFromOpen);
    ASSERT_EQ(0, ticksReverse);
    // Try at half-way mark, no reverse ticks.
    ticksFromOpen = tfo2 / 2;
    ticksReverse = 0;
    ASSERT_EQ(50, cp.computePosition(ticksFromOpen, ticksReverse));
    ASSERT_EQ(tfo2/2, ticksFromOpen);
    ASSERT_EQ(0, ticksReverse);
    // Try at half-way mark with just one reverse tick (nothing should change).
    ticksFromOpen = tfo2 / 2;
    ticksReverse = 1;
    ASSERT_EQ(50, cp.computePosition(ticksFromOpen, ticksReverse));
    ASSERT_EQ(tfo2/2, ticksFromOpen);
    ASSERT_EQ(1, ticksReverse);
    // Try at half-way mark with a big-enough block of reverse ticks to be significant.
    ticksFromOpen = tfo2 / 2;
    ticksReverse = cp.getTfctoSmall();
    ASSERT_EQ(51, cp.computePosition(ticksFromOpen, ticksReverse));
    ASSERT_EQ(tfo2/2 - cp.getTfotcSmall(), ticksFromOpen);
    ASSERT_EQ(0, ticksReverse);
  // DHD20151025: one set of actual measurements during calibration.
  //    ticksFromOpenToClosed: 1529
  //    ticksFromClosedToOpen: 1295
}

class DummyHardwareDriver : public OTRadValve::HardwareMotorDriverInterface
  {
  public:
    // Detect if end-stop is reached or motor current otherwise very high.
    virtual bool isCurrentHigh(OTRadValve::HardwareMotorDriverInterface::motor_drive mdir = motorDriveOpening) const override { return(currentHigh); }
    // isCurrentHigh() returns this value.
    bool currentHigh;
    DummyHardwareDriver() : currentHigh(false) { }
    virtual void motorRun(uint8_t maxRunTicks, motor_drive dir, OTRadValve::HardwareMotorDriverInterfaceCallbackHandler &callback) override
      { }
  };

// Always claims to be at the start of a major cycle.
static uint8_t dummyGetSubCycleTime() { return(0); }

// Test that direct abstract motor drive logic is constructable and minimally sane..
//
// Adapted 2016/10/18 from test_VALVEMODEL.ino testCurrentSenseValveMotorDirect().
TEST(CurrentSenseValveMotorDirect,bascis)
{
    const uint8_t subcycleTicksRoundedDown_ms = 7; // For REV7: OTV0P2BASE::SUBCYCLE_TICK_MS_RD.
    const uint8_t gsct_max = 255; // For REV7: OTV0P2BASE::GSCT_MAX.
    const uint8_t minimumMotorRunupTicks = 4; // For REV7: OTRadValve::ValveMotorDirectV1HardwareDriverBase::minMotorRunupTicks.
    DummyHardwareDriver dhw;
    OTRadValve::CurrentSenseValveMotorDirect csvmd1(&dhw, dummyGetSubCycleTime,
        OTRadValve::CurrentSenseValveMotorDirect::computeMinMotorDRTicks(subcycleTicksRoundedDown_ms),
        OTRadValve::CurrentSenseValveMotorDirect::computeSctAbsLimit(subcycleTicksRoundedDown_ms,
                                                                     gsct_max,
                                                                     minimumMotorRunupTicks));

    // POWER UP
    // Whitebox test of internal state: should be init.
    ASSERT_EQ(OTRadValve::CurrentSenseValveMotorDirect::init, csvmd1.getState());
    // Verify NOT marked as in normal run state immediately upon initialisation.
    ASSERT_TRUE(!csvmd1.isInNormalRunState());
    // Verify NOT marked as in error state immediately upon initialisation.
    ASSERT_TRUE(!csvmd1.isInErrorState());
    // Target % open must start off in a sensible state; fully-closed is good.
    ASSERT_EQ(0, csvmd1.getTargetPC());
    // Until calibration has been successfully run, this should be in non-proportional mode.
    ASSERT_TRUE(csvmd1.inNonProprtionalMode());
    // Nothing passed in requires deferral of (re)calibration.
    ASSERT_FALSE(csvmd1.shouldDeferCalibration());
}

class SVL final : public OTV0P2BASE::SupplyVoltageLow
    {
    public:
      SVL() { setAllLowFlags(false); }
      void setAllLowFlags(const bool f) { isLow = f; isVeryLow = f; }
      virtual uint16_t get() const override { return(0); }
      virtual uint16_t read() override { return(0); }
    };
// State of ambient lighting.
static bool _isDark;
static bool isDark() { return(_isDark); }

// Test that logic for potentially deferring (re)calibration is correct.
TEST(CurrentSenseValveMotorDirect,calibrationDeferral)
{
    const uint8_t subcycleTicksRoundedDown_ms = 7; // For REV7: OTV0P2BASE::SUBCYCLE_TICK_MS_RD.
    const uint8_t gsct_max = 255; // For REV7: OTV0P2BASE::GSCT_MAX.
    const uint8_t minimumMotorRunupTicks = 4; // For REV7: OTRadValve::ValveMotorDirectV1HardwareDriverBase::minMotorRunupTicks.
    DummyHardwareDriver dhw;
    SVL svl;
    _isDark = false;
    OTRadValve::CurrentSenseValveMotorDirect csvmd1(&dhw, dummyGetSubCycleTime,
        OTRadValve::CurrentSenseValveMotorDirect::computeMinMotorDRTicks(subcycleTicksRoundedDown_ms),
        OTRadValve::CurrentSenseValveMotorDirect::computeSctAbsLimit(subcycleTicksRoundedDown_ms,
                                                                     gsct_max,
                                                                     minimumMotorRunupTicks),
        &svl,
        isDark);
    // Nothing yet requires deferral of (re)calibration.
    ASSERT_FALSE(csvmd1.shouldDeferCalibration());
    svl.setAllLowFlags(true);
    // Low supply voltage requires deferral of (re)calibration.
    ASSERT_TRUE(csvmd1.shouldDeferCalibration());
    svl.setAllLowFlags(false);
    ASSERT_FALSE(csvmd1.shouldDeferCalibration());
    _isDark = true;
    // Low light level requires deferral of (re)calibration.
    ASSERT_TRUE(csvmd1.shouldDeferCalibration());
    svl.setAllLowFlags(true);
    ASSERT_TRUE(csvmd1.shouldDeferCalibration());
    _isDark = false;
    svl.setAllLowFlags(false);
    // Nothing requires deferral of (re)calibration.
    ASSERT_FALSE(csvmd1.shouldDeferCalibration());
}

<|endoftext|>


<commit_before>// $Id: sample.cpp 793 2008-08-17 14:33:30Z glandrum $
//
//  Copyright (C) 2009 Greg Landrum
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//

#include <GraphMol/RDKitBase.h>
#include <GraphMol/SmilesParse/SmilesParse.h>
#include <GraphMol/Depictor/RDDepictor.h>
#include <Demos/RDKit/Draw/MolDrawing.h>

#include <RDGeneral/RDLog.h>
#include <vector>

using namespace RDKit;
void DrawDemo(){
  RWMol *mol=SmilesToMol("Clc1c(C#N)cc(C(=O)NCc2sccc2)cc1");
  //RWMol *mol=SmilesToMol("c1ncncn1");
  RDKit::MolOps::Kekulize(*mol);

  // generate the 2D coordinates:
  RDDepict::compute2DCoords(*mol);
  std::vector<int> drawing=RDKit::Drawing::DrawMol(*mol);

  std::cout<<"var codes=[";
  std::copy(drawing.begin(),drawing.end(),std::ostream_iterator<int>(std::cout,","));
  std::cout<<"];"<<std::endl;

  delete mol;
}

int
main(int argc, char *argv[])
{
  RDLog::InitLogs();
  DrawDemo();
}
<commit_msg>initial pass at very crude svg translation<commit_after>// $Id: sample.cpp 793 2008-08-17 14:33:30Z glandrum $
//
//  Copyright (C) 2009 Greg Landrum
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//

#include <GraphMol/RDKitBase.h>
#include <GraphMol/SmilesParse/SmilesParse.h>
#include <GraphMol/Depictor/RDDepictor.h>
#include <Demos/RDKit/Draw/MolDrawing.h>

#include <RDGeneral/RDLog.h>
#include <vector>
#include <sstream>

using namespace RDKit;

std::string getColor(int atNum){
  static std::map<int,std::string> colors;
  if(colors.empty()){
    colors[7]="#0000FF";
    colors[8]="#FF0000";
    colors[9]="#33CCCC";
    colors[15]="#FF7F00";
    colors[16]="#CCCC00";
    colors[16]="#00CC00";
    colors[35]="#7F4C19";
    colors[0]="#7F7F7F";
  }
  std::string res="#000000";
  if(colors.find(atNum)!=colors.end()) res= colors[atNum];
  return res;
}
void drawLine(std::vector<int>::const_iterator &pos,std::ostringstream &sstr){
      pos+=4;
      sstr<<"<svg:path ";
      sstr<<"d='M "<<*pos<<","<<*(pos+1)<<" "<<*(pos+2)<<","<<*(pos+3)<<"' ";
      pos+=4;
      sstr<<"style='fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:3px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1'";
      sstr<<" />\n";
}
void drawAtom(std::vector<int>::const_iterator &pos,std::ostringstream &sstr){
  int atNum=*pos++;
  int xp=*pos++;
  int yp=*pos++;
  int slen=*pos++;
  std::string label="";
  for(unsigned int i=0;i<slen;++i){
    label+=(char)*pos++;
  }
  sstr<<"<svg:text x='"<<xp<<"' y='"<<yp<<"'";
  sstr<<" style='font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill-opacity:1;stroke:none;font-family:Sans;text-anchor:middle;baseline-shift:sub"<<";fill:"<<getColor(atNum)<<"'";
  sstr<<">";
  sstr<<"<svg:tspan x='"<<xp<<"' y='"<<yp<<"'>";
  sstr<<label<<"</svg:tspan>";
  sstr<<"</svg:text>\n";
}

std::string ToSVG(const std::vector<int> &drawing){
  std::ostringstream sstr;
  sstr<<"<?xml version='1.0' encoding='iso-8859-1'?>\n";
  int width=300,height=300;
  std::vector<int>::const_iterator pos=drawing.begin()+2;
  TEST_ASSERT(*pos==Drawing::BOUNDS);
  pos+=3;
  width = *pos++;
  height = *pos++;
  sstr << "<svg:svg version='1.1' baseProfile='full'\n      \
        xmlns:svg='http://www.w3.org/2000/svg'\n                \
        xmlns:xlink='http://www.w3.org/1999/xlink'\n          \
        xml:space='preserve'\n";
  sstr<<"width='"<<width<<"px' height='"<<height<<"px' >\n";
  sstr<<"<svg:g transform='translate("<<width*.05<<","<<height*.05<<") scale(.9,.9)'>";
  while(pos!=drawing.end()){
    int token=*pos++;
    switch(token){
    case Drawing::LINE:
      drawLine(pos,sstr);
      break;
    case Drawing::ATOM:
      drawAtom(pos,sstr);
      break;
    default:
      std::cerr<<"unrecognized token: "<<token<<std::endl;
    }
  }

  sstr<<"</svg:g></svg:svg>";
  return sstr.str();
}

void DrawDemo(){
  RWMol *mol=SmilesToMol("Clc1c(C#N)cc(C(=O)NCc2sccc2)cc1");
  //RWMol *mol=SmilesToMol("c1ncncn1");
  RDKit::MolOps::Kekulize(*mol);

  // generate the 2D coordinates:
  RDDepict::compute2DCoords(*mol);
  std::vector<int> drawing=RDKit::Drawing::DrawMol(*mol);

  std::cerr<<"var codes=[";
  std::copy(drawing.begin(),drawing.end(),std::ostream_iterator<int>(std::cerr,","));
  std::cerr<<"];"<<std::endl;

  std::string svg=ToSVG(drawing);
  std::cout<<svg<<std::endl;

  delete mol;
}

int
main(int argc, char *argv[])
{
  RDLog::InitLogs();
  DrawDemo();
}
<|endoftext|>