<commit_before>#include "DateTimeAxis.hpp"

#include <QPainter>
#include <QDateTime>

namespace cutehmi {
namespace charts {

constexpr DateTimeAxis::interval_t DateTimeAxis::INITIAL_TICK_INTERVAL;
constexpr DateTimeAxis::interval_t DateTimeAxis::INITIAL_MINOR_TICK_INTERVAL;

DateTimeAxis::DateTimeAxis(QQuickItem * parent):
	LabeledAxis(parent),
	m_tickInterval(INITIAL_TICK_INTERVAL),
	m_minorTickInterval(INITIAL_MINOR_TICK_INTERVAL)
{
	switch (orientation()) {
		case Qt::Horizontal:
			connect(this, & DateTimeAxis::widthChanged, this, & DateTimeAxis::recalcRatio);
			break;
		case Qt::Vertical:
			connect(this, & DateTimeAxis::heightChanged, this, & DateTimeAxis::recalcRatio);
			break;
		default:
			qWarning("Unsupported orientation: %d.", orientation());
	}
	connect(this, & DateTimeAxis::fromChanged, this, & DateTimeAxis::recalcRatio);
	connect(this, & DateTimeAxis::toChanged, this, & DateTimeAxis::recalcRatio);

	connect(this, & ValueAxis::plotAreaChanged, this, & DateTimeAxis::connectPlotArea);

	connect(this, & DateTimeAxis::orientationChanged, this, & DateTimeAxis::connectOrientation);

	// Connect signals that should trigger paint.
	connect(this, & DateTimeAxis::colorChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::textColorChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::fontChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::thicknessChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::fromChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::toChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::tickIntervalChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::minorTickIntervalChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::axisPositionChanged, this, & QQuickItem::update);
	connect(this, & ValueAxis::plotAreaChanged, this, & QQuickItem::update);
}

DateTimeAxis::interval_t DateTimeAxis::tickInterval() const
{
	return m_tickInterval;
}

void DateTimeAxis::setTickInterval(interval_t interval)
{
	if (m_tickInterval != interval) {
		m_tickInterval = interval;
		emit tickIntervalChanged();
	}
}

DateTimeAxis::interval_t DateTimeAxis::minorTickInterval() const
{
	return m_minorTickInterval;
}

void DateTimeAxis::setMinorTickInterval(interval_t interval)
{
	if (m_minorTickInterval != interval) {
		m_minorTickInterval = interval;
		emit minorTickIntervalChanged();
	}
}

qreal DateTimeAxis::mapToPlotArea(qreal value) const
{
	switch (orientation()) {
		case Qt::Horizontal:
			return (value - from()) * m_screenScaleRatio;
		case Qt::Vertical:
			return (to() - value) * m_screenScaleRatio;
		default:
			return qQNaN();
	}
}

void DateTimeAxis::walkTicks(std::function<void(qreal tick)> callback) const
{
	QDateTime curTickDT = roundTick(from(), m_tickInterval);
	QDateTime endDate = QDateTime::fromMSecsSinceEpoch(to());

	while (curTickDT < endDate) {
		callback(curTickDT.toMSecsSinceEpoch());
		advanceTickDateTime(curTickDT, m_tickInterval);
	}
}

void DateTimeAxis::walkMinorTicks(std::function<void(qreal tick)> callback) const
{
	QDateTime curTickDT = roundTick(from(), m_minorTickInterval);
	QDateTime endDate = QDateTime::fromMSecsSinceEpoch(to());

	while (curTickDT < endDate) {
		callback(curTickDT.toMSecsSinceEpoch());
		advanceTickDateTime(curTickDT, m_minorTickInterval);
	}
}

void DateTimeAxis::walkTickLabels(std::function<void(qreal labelPos)> callback) const
{
	QDateTime curTickDT = roundTick(from(), m_tickInterval);
	// Put labels in between ticks.
	if ((m_tickInterval == DAYS) ||
		(m_tickInterval == WEEKS) ||
		(m_tickInterval == MONTHS) ||
		(m_tickInterval == YEARS)) {
		QDateTime nextTickDT = curTickDT;
		advanceTickDateTime(nextTickDT, m_tickInterval);
		curTickDT = curTickDT.addMSecs((nextTickDT.toMSecsSinceEpoch() - curTickDT.toMSecsSinceEpoch()) / 2.0);
	}
	QDateTime endDate = QDateTime::fromMSecsSinceEpoch(to());

	while (curTickDT < endDate) {
		callback(curTickDT.toMSecsSinceEpoch());
		advanceTickDateTime(curTickDT, m_tickInterval);
	}
}

QString DateTimeAxis::tickToString(qreal tick) const
{
	QDateTime tickDT = QDateTime::fromMSecsSinceEpoch(tick);
	// Restore original tick position of the labels in between ticks.
	if ((m_tickInterval == DAYS) ||
		(m_tickInterval == WEEKS) ||
		(m_tickInterval == MONTHS) ||
		(m_tickInterval == YEARS)) {
		QDateTime nextTickDT = tickDT;
		advanceTickDateTime(nextTickDT, m_tickInterval);
		tickDT = tickDT.addMSecs(-((nextTickDT.toMSecsSinceEpoch() - tickDT.toMSecsSinceEpoch()) / 2.0));
	}

	switch (m_tickInterval) {
		case MILLISECONDS:
			return tickDT.toString("zzz");
		case SECONDS:
			return tickDT.toString("ss");
		case MINUTES:
			return tickDT.toString("mm");
		case HOURS:
			return tickDT.toString("hh:mm");
		case DAYS:
			return tickDT.toString("ddd");
		case WEEKS:
			return QString::number(tickDT.date().weekNumber());
		case MONTHS:
			return tickDT.toString("MMM");
		case YEARS:
			return tickDT.toString("yyyy");
	}
	return QDateTime::fromMSecsSinceEpoch(tick).toString();
}

QString DateTimeAxis::boundToString(qreal tick) const
{
	return QDateTime::fromMSecsSinceEpoch(tick).toString();
}

void DateTimeAxis::connectPlotArea(PlotArea * oldPlotArea)
{
	QQuickItem * oldEmitter = oldPlotArea ? static_cast<QQuickItem * >(oldPlotArea) : static_cast<QQuickItem * >(this);
	QQuickItem * newEmitter = plotArea() ? static_cast<QQuickItem * >(plotArea()) : static_cast<QQuickItem *>(this);
	switch (orientation()) {
		case Qt::Horizontal:
			disconnect(oldEmitter, & QQuickItem::widthChanged, this, & DateTimeAxis::recalcRatio);
			connect(newEmitter, & QQuickItem::widthChanged, this, & DateTimeAxis::recalcRatio);
			break;
		case Qt::Vertical:
			disconnect(oldEmitter, & QQuickItem::heightChanged, this, & DateTimeAxis::recalcRatio);
			connect(newEmitter, & QQuickItem::heightChanged, this, & DateTimeAxis::recalcRatio);
			break;
	}
	recalcRatio();
}

void DateTimeAxis::recalcRatio()
{
	switch (orientation()) {
		case Qt::Horizontal:
			m_screenScaleRatio = (plotAreaWidth()) / (to() - from());
			break;
		case Qt::Vertical:
			m_screenScaleRatio = (plotAreaHeight()) / (to() - from());
			break;
		default:
			m_screenScaleRatio = 0.0;
	}
}

void DateTimeAxis::connectOrientation()
{
	QQuickItem * emitter = plotArea() ? static_cast<QQuickItem * >(plotArea()) : static_cast<QQuickItem *>(this);
	switch (orientation()) {
		case Qt::Horizontal:
			disconnect(emitter, & QQuickItem::heightChanged, this, & DateTimeAxis::recalcRatio);
			connect(emitter, & QQuickItem::widthChanged, this, & DateTimeAxis::recalcRatio);
			break;
		case Qt::Vertical:
			disconnect(emitter, & QQuickItem::widthChanged, this, & DateTimeAxis::recalcRatio);
			connect(emitter, & QQuickItem::heightChanged, this, & DateTimeAxis::recalcRatio);
			break;
		default:
			qWarning("Unsupported orientation: %d.", orientation());
	}
	recalcRatio();
}

qreal DateTimeAxis::tickMax() const
{
	return (orientation() == Qt::Horizontal) ? width() : height();
}

qreal DateTimeAxis::approxTickIntervalToMSec(interval_t interval) const
{
	qreal result = 0.0;
	switch (interval) {
		case MILLISECONDS:
			return 1.0;
		case SECONDS:
			result = 1000.0;
		case MINUTES:
			result *= 60.0;
		case HOURS:
			result *= 60.0;
		case DAYS:
			result *= 24.0;
		case WEEKS:
			result *= 7.0;
		case MONTHS:
			result *= 30.0;
		case YEARS:
			result *= 365.0;
	}
	return result;
}

QDateTime DateTimeAxis::roundTick(qreal tick, interval_t tickInterval) const
{
	QDate d = QDateTime::fromMSecsSinceEpoch(tick).date();
	QTime t = QDateTime::fromMSecsSinceEpoch(tick).time();
	QDateTime roundTick;
	switch (tickInterval) {
		case MILLISECONDS:
			roundTick = QDateTime::fromMSecsSinceEpoch(from());
			break;
		case SECONDS:
			roundTick = QDateTime(QDate(d.year(), d.month(), d.day()), QTime(t.hour(), t.minute(), t.second()));
			roundTick = roundTick.addSecs(1);
			break;
		case MINUTES:
			roundTick = QDateTime(QDate(d.year(), d.month(), d.day()), QTime(t.hour(), t.minute()));
			roundTick = roundTick.addSecs(60);
			break;
		case HOURS:
			roundTick = QDateTime(QDate(d.year(), d.month(), d.day()), QTime(t.hour(), 0));
			roundTick = roundTick.addSecs(60 * 60);
			break;
		case DAYS:
			roundTick = QDateTime(QDate(d.year(), d.month(), d.day()));
			roundTick = roundTick.addDays(1);
			break;
		case WEEKS:
			roundTick = QDateTime(QDate(d.year(), d.month(), d.day() + 7 - d.dayOfWeek()));
			break;
		case MONTHS:
			roundTick = QDateTime(QDate(d.year(), d.month(), 0));
			roundTick = roundTick.addMonths(1);
			break;
		case YEARS:
			roundTick = QDateTime(QDate(d.year(), 0, 0));
			roundTick = roundTick.addYears(1);
			break;
	}
	return roundTick;
}

void DateTimeAxis::advanceTickDateTime(QDateTime & tickDT, interval_t tickInterval) const
{
	switch (tickInterval) {
		case MILLISECONDS:
			tickDT = tickDT.addMSecs(1);
			break;
		case SECONDS:
			tickDT = tickDT.addSecs(1);
			break;
		case MINUTES:
			tickDT = tickDT.addSecs(60);
			break;
		case HOURS:
			tickDT = tickDT.addSecs(60 * 60);
			break;
		case DAYS:
			tickDT = tickDT.addDays(1);
			break;
		case WEEKS:
			tickDT = tickDT.addDays(7);
			break;
		case MONTHS:
			tickDT = tickDT.addMonths(1);
			break;
		case YEARS:
			tickDT = tickDT.addYears(1);
			break;
	}
}

}
}
<commit_msg>Remove unnecessary restore to original tick position.<commit_after>#include "DateTimeAxis.hpp"

#include <QPainter>
#include <QDateTime>

namespace cutehmi {
namespace charts {

constexpr DateTimeAxis::interval_t DateTimeAxis::INITIAL_TICK_INTERVAL;
constexpr DateTimeAxis::interval_t DateTimeAxis::INITIAL_MINOR_TICK_INTERVAL;

DateTimeAxis::DateTimeAxis(QQuickItem * parent):
	LabeledAxis(parent),
	m_tickInterval(INITIAL_TICK_INTERVAL),
	m_minorTickInterval(INITIAL_MINOR_TICK_INTERVAL)
{
	switch (orientation()) {
		case Qt::Horizontal:
			connect(this, & DateTimeAxis::widthChanged, this, & DateTimeAxis::recalcRatio);
			break;
		case Qt::Vertical:
			connect(this, & DateTimeAxis::heightChanged, this, & DateTimeAxis::recalcRatio);
			break;
		default:
			qWarning("Unsupported orientation: %d.", orientation());
	}
	connect(this, & DateTimeAxis::fromChanged, this, & DateTimeAxis::recalcRatio);
	connect(this, & DateTimeAxis::toChanged, this, & DateTimeAxis::recalcRatio);

	connect(this, & ValueAxis::plotAreaChanged, this, & DateTimeAxis::connectPlotArea);

	connect(this, & DateTimeAxis::orientationChanged, this, & DateTimeAxis::connectOrientation);

	// Connect signals that should trigger paint.
	connect(this, & DateTimeAxis::colorChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::textColorChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::fontChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::thicknessChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::fromChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::toChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::tickIntervalChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::minorTickIntervalChanged, this, & QQuickItem::update);
	connect(this, & DateTimeAxis::axisPositionChanged, this, & QQuickItem::update);
	connect(this, & ValueAxis::plotAreaChanged, this, & QQuickItem::update);
}

DateTimeAxis::interval_t DateTimeAxis::tickInterval() const
{
	return m_tickInterval;
}

void DateTimeAxis::setTickInterval(interval_t interval)
{
	if (m_tickInterval != interval) {
		m_tickInterval = interval;
		emit tickIntervalChanged();
	}
}

DateTimeAxis::interval_t DateTimeAxis::minorTickInterval() const
{
	return m_minorTickInterval;
}

void DateTimeAxis::setMinorTickInterval(interval_t interval)
{
	if (m_minorTickInterval != interval) {
		m_minorTickInterval = interval;
		emit minorTickIntervalChanged();
	}
}

qreal DateTimeAxis::mapToPlotArea(qreal value) const
{
	switch (orientation()) {
		case Qt::Horizontal:
			return (value - from()) * m_screenScaleRatio;
		case Qt::Vertical:
			return (to() - value) * m_screenScaleRatio;
		default:
			return qQNaN();
	}
}

void DateTimeAxis::walkTicks(std::function<void(qreal tick)> callback) const
{
	QDateTime curTickDT = roundTick(from(), m_tickInterval);
	QDateTime endDate = QDateTime::fromMSecsSinceEpoch(to());

	while (curTickDT < endDate) {
		callback(curTickDT.toMSecsSinceEpoch());
		advanceTickDateTime(curTickDT, m_tickInterval);
	}
}

void DateTimeAxis::walkMinorTicks(std::function<void(qreal tick)> callback) const
{
	QDateTime curTickDT = roundTick(from(), m_minorTickInterval);
	QDateTime endDate = QDateTime::fromMSecsSinceEpoch(to());

	while (curTickDT < endDate) {
		callback(curTickDT.toMSecsSinceEpoch());
		advanceTickDateTime(curTickDT, m_minorTickInterval);
	}
}

void DateTimeAxis::walkTickLabels(std::function<void(qreal labelPos)> callback) const
{
	QDateTime curTickDT = roundTick(from(), m_tickInterval);
	// Put labels in between ticks.
	if ((m_tickInterval == DAYS) ||
		(m_tickInterval == WEEKS) ||
		(m_tickInterval == MONTHS) ||
		(m_tickInterval == YEARS)) {
		QDateTime nextTickDT = curTickDT;
		advanceTickDateTime(nextTickDT, m_tickInterval);
		curTickDT = curTickDT.addMSecs((nextTickDT.toMSecsSinceEpoch() - curTickDT.toMSecsSinceEpoch()) / 2.0);
	}
	QDateTime endDate = QDateTime::fromMSecsSinceEpoch(to());

	while (curTickDT < endDate) {
		callback(curTickDT.toMSecsSinceEpoch());
		advanceTickDateTime(curTickDT, m_tickInterval);
	}
}

QString DateTimeAxis::tickToString(qreal tick) const
{
	QDateTime tickDT = QDateTime::fromMSecsSinceEpoch(tick);

	switch (m_tickInterval) {
		case MILLISECONDS:
			return tickDT.toString("zzz");
		case SECONDS:
			return tickDT.toString("ss");
		case MINUTES:
			return tickDT.toString("mm");
		case HOURS:
			return tickDT.toString("hh:mm");
		case DAYS:
			return tickDT.toString("ddd");
		case WEEKS:
			return QString::number(tickDT.date().weekNumber());
		case MONTHS:
			return tickDT.toString("MMM");
		case YEARS:
			return tickDT.toString("yyyy");
	}
	return QDateTime::fromMSecsSinceEpoch(tick).toString();
}

QString DateTimeAxis::boundToString(qreal tick) const
{
	return QDateTime::fromMSecsSinceEpoch(tick).toString();
}

void DateTimeAxis::connectPlotArea(PlotArea * oldPlotArea)
{
	QQuickItem * oldEmitter = oldPlotArea ? static_cast<QQuickItem * >(oldPlotArea) : static_cast<QQuickItem * >(this);
	QQuickItem * newEmitter = plotArea() ? static_cast<QQuickItem * >(plotArea()) : static_cast<QQuickItem *>(this);
	switch (orientation()) {
		case Qt::Horizontal:
			disconnect(oldEmitter, & QQuickItem::widthChanged, this, & DateTimeAxis::recalcRatio);
			connect(newEmitter, & QQuickItem::widthChanged, this, & DateTimeAxis::recalcRatio);
			break;
		case Qt::Vertical:
			disconnect(oldEmitter, & QQuickItem::heightChanged, this, & DateTimeAxis::recalcRatio);
			connect(newEmitter, & QQuickItem::heightChanged, this, & DateTimeAxis::recalcRatio);
			break;
	}
	recalcRatio();
}

void DateTimeAxis::recalcRatio()
{
	switch (orientation()) {
		case Qt::Horizontal:
			m_screenScaleRatio = (plotAreaWidth()) / (to() - from());
			break;
		case Qt::Vertical:
			m_screenScaleRatio = (plotAreaHeight()) / (to() - from());
			break;
		default:
			m_screenScaleRatio = 0.0;
	}
}

void DateTimeAxis::connectOrientation()
{
	QQuickItem * emitter = plotArea() ? static_cast<QQuickItem * >(plotArea()) : static_cast<QQuickItem *>(this);
	switch (orientation()) {
		case Qt::Horizontal:
			disconnect(emitter, & QQuickItem::heightChanged, this, & DateTimeAxis::recalcRatio);
			connect(emitter, & QQuickItem::widthChanged, this, & DateTimeAxis::recalcRatio);
			break;
		case Qt::Vertical:
			disconnect(emitter, & QQuickItem::widthChanged, this, & DateTimeAxis::recalcRatio);
			connect(emitter, & QQuickItem::heightChanged, this, & DateTimeAxis::recalcRatio);
			break;
		default:
			qWarning("Unsupported orientation: %d.", orientation());
	}
	recalcRatio();
}

qreal DateTimeAxis::tickMax() const
{
	return (orientation() == Qt::Horizontal) ? width() : height();
}

qreal DateTimeAxis::approxTickIntervalToMSec(interval_t interval) const
{
	qreal result = 0.0;
	switch (interval) {
		case MILLISECONDS:
			return 1.0;
		case SECONDS:
			result = 1000.0;
		case MINUTES:
			result *= 60.0;
		case HOURS:
			result *= 60.0;
		case DAYS:
			result *= 24.0;
		case WEEKS:
			result *= 7.0;
		case MONTHS:
			result *= 30.0;
		case YEARS:
			result *= 365.0;
	}
	return result;
}

QDateTime DateTimeAxis::roundTick(qreal tick, interval_t tickInterval) const
{
	QDate d = QDateTime::fromMSecsSinceEpoch(tick).date();
	QTime t = QDateTime::fromMSecsSinceEpoch(tick).time();
	QDateTime roundTick;
	switch (tickInterval) {
		case MILLISECONDS:
			roundTick = QDateTime::fromMSecsSinceEpoch(from());
			break;
		case SECONDS:
			roundTick = QDateTime(QDate(d.year(), d.month(), d.day()), QTime(t.hour(), t.minute(), t.second()));
			roundTick = roundTick.addSecs(1);
			break;
		case MINUTES:
			roundTick = QDateTime(QDate(d.year(), d.month(), d.day()), QTime(t.hour(), t.minute()));
			roundTick = roundTick.addSecs(60);
			break;
		case HOURS:
			roundTick = QDateTime(QDate(d.year(), d.month(), d.day()), QTime(t.hour(), 0));
			roundTick = roundTick.addSecs(60 * 60);
			break;
		case DAYS:
			roundTick = QDateTime(QDate(d.year(), d.month(), d.day()));
			roundTick = roundTick.addDays(1);
			break;
		case WEEKS:
			roundTick = QDateTime(QDate(d.year(), d.month(), d.day() + 7 - d.dayOfWeek()));
			break;
		case MONTHS:
			roundTick = QDateTime(QDate(d.year(), d.month(), 0));
			roundTick = roundTick.addMonths(1);
			break;
		case YEARS:
			roundTick = QDateTime(QDate(d.year(), 0, 0));
			roundTick = roundTick.addYears(1);
			break;
	}
	return roundTick;
}

void DateTimeAxis::advanceTickDateTime(QDateTime & tickDT, interval_t tickInterval) const
{
	switch (tickInterval) {
		case MILLISECONDS:
			tickDT = tickDT.addMSecs(1);
			break;
		case SECONDS:
			tickDT = tickDT.addSecs(1);
			break;
		case MINUTES:
			tickDT = tickDT.addSecs(60);
			break;
		case HOURS:
			tickDT = tickDT.addSecs(60 * 60);
			break;
		case DAYS:
			tickDT = tickDT.addDays(1);
			break;
		case WEEKS:
			tickDT = tickDT.addDays(7);
			break;
		case MONTHS:
			tickDT = tickDT.addMonths(1);
			break;
		case YEARS:
			tickDT = tickDT.addYears(1);
			break;
	}
}

}
}
<|endoftext|>


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

#include <iomanip>
#include <sstream>

#include "mathfu/constants.h"
#include "motive/anim.h"
#include "motive/engine.h"
#include "motive/init.h"
#include "motive/math/angle.h"
#include "motive/math/bulk_spline_evaluator.h"

using motive::Angle;
using motive::kPi;
using mathfu::AffineTransform;
using mathfu::mat4;
using mathfu::vec4;

namespace motive {

// static
static const MatrixOpArray kEmptyOps(0);

class RigData {
 public:
  explicit RigData(const RigInit& init, MotiveTime start_time,
                   MotiveEngine* engine)
      : motivators_(nullptr),
        global_transforms_(nullptr),
        defining_anim_(&init.defining_anim()),
        current_anim_(nullptr),
        end_time_(start_time) {
    const BoneIndex num_bones = defining_anim_->NumBones();

    // Visual Studio 2010 does not like std::vectors of mat4, since they are
    // a 16-byte aligned type. Use plain old arrays instead.
    motivators_ = new MatrixMotivator4f[num_bones];
    global_transforms_ = new AffineTransform[num_bones];

    // Initialize the motivators that drive the local transforms.
    for (BoneIndex i = 0; i < num_bones; ++i) {
      const MatrixOpArray& ops = defining_anim_->Anim(i).ops();
      motivators_[i].Initialize(MatrixInit(ops), engine);
    }

    // Initialize global transforms to default pose.
    // These will get overridden the first time AdvanceFrame() is called, but
    // we initialize them nicely anyway.
    UpdateGlobalTransforms();
  }

  ~RigData() {
    delete[] motivators_;
    motivators_ = nullptr;

    delete[] global_transforms_;
    global_transforms_ = nullptr;
  }

  void BlendToAnim(const RigAnim& anim, const motive::SplinePlayback& playback,
                   MotiveTime start_time) {
    end_time_ = start_time + anim.end_time();

    // When animation has only one bone, or mesh has only one bone,
    // we simply animate the root node only.
    const int anim_num_bones = anim.NumBones();
    const int defining_num_bones = NumBones();
    assert(defining_num_bones == 1 || anim_num_bones == 1 ||
           RigInit::MatchesHierarchy(anim, *defining_anim_));

    // Update the motivators to blend to our new values.
    for (BoneIndex i = 0; i < defining_num_bones; ++i) {
      const MatrixOpArray& ops =
          i >= anim_num_bones ? kEmptyOps : anim.Anim(i).ops();
      motivators_[i].BlendToOps(ops, playback);
    }

    // Remember the currently playing animation, for debugging purposes.
    current_anim_ = &anim;
  }

  const RigAnim* current_anim() const { return current_anim_; }

  void SetPlaybackRate(float playback_rate) {
    // Update the motivators to have the new playback rate.
    // TODO: Do this in bulk.
    const int defining_num_bones = NumBones();
    for (BoneIndex i = 0; i < defining_num_bones; ++i) {
      motivators_[i].SetPlaybackRate(playback_rate);
    }
  }

  MotiveTime TimeRemaining() const {
    if (end_time_ == kMotiveTimeEndless) {
      return kMotiveTimeEndless;
    }
    MotiveTime time = 0;
    const int defining_num_bones = NumBones();
    for (BoneIndex i = 0; i < defining_num_bones; ++i) {
      time = std::max(time, motivators_[i].TimeRemaining());
    }
    return time;
  }

  void UpdateGlobalTransforms() {
    CalculateGlobalTransforms(global_transforms_);
  }

  const AffineTransform* GlobalTransforms() const { return global_transforms_; }

  BoneIndex NumBones() const { return defining_anim_->NumBones(); }

  MotiveTime end_time() const { return end_time_; }

  const RigAnim* defining_anim() const { return defining_anim_; }

  void ChildValuesForDebugging(std::vector<float>* values) const {
    values->resize(defining_anim_->NumOps());

    int k = 0;
    const int defining_num_bones = NumBones();
    for (BoneIndex i = 0; i < defining_num_bones; ++i) {
      const MotiveChildIndex num_children = motivators_[i].NumChildren();
      for (MotiveChildIndex j = 0; j < num_children; ++j) {
        (*values)[k++] = motivators_[i].ChildValue1f(j);
      }
    }
  }

  std::string CsvHeaderForDebugging() const {
    std::ostringstream oss;
    oss << "animation name,time," << defining_anim_->CsvHeaderForDebugging(0)
        << std::endl;
    oss << ",," << defining_anim_->CsvHeaderForDebugging(1) << std::endl;
    return oss.str();
  }

  std::string CsvValuesForDebugging(MotiveTime current_time) const {
    std::vector<float> values;
    ChildValuesForDebugging(&values);

    std::ostringstream oss;
    const MotiveTime anim_time =
        current_time - end_time_ + current_anim_->end_time();
    oss << current_anim_->anim_name() << ',' << anim_time << ',';

    int k = 0;
    const int defining_num_bones = NumBones();
    for (BoneIndex i = 0; i < defining_num_bones; ++i) {
      const MatrixOpArray::OpVector& ops = defining_anim_->Anim(i).ops().ops();
      for (size_t j = 0; j < ops.size(); ++j) {
        const float multiplier = RotateOp(ops[j].type) ? 180.0f / kPi : 1.0f;
        oss << multiplier * values[k] << ',';
        k++;
      }
    }

    return oss.str();
  }

  std::string LocalTransformsForDebugging(BoneIndex bone,
                                          MotiveTime time) const {
    const BoneIndex* bone_parents = defining_anim_->bone_parents();

    // Output four lines: one per row of matrix.
    std::ostringstream oss;
    oss.precision(2);
    oss << std::fixed << std::right;

    // Output header
    const MotiveTime time_until_end = end_time_ - time;
    const MotiveTime time_since_start =
        current_anim_->end_time() - time_until_end;
    oss << current_anim_->anim_name() << " at time " << time_since_start << " ("
        << (time_since_start * 24.0f / 1000.0f) << " @24fps)" << std::endl;
    for (BoneIndex idx = bone; idx != kInvalidBoneIdx;
         idx = bone_parents[idx]) {
      // Output the bone's name.
      const char* bone_name = defining_anim_->BoneName(idx);
      oss << bone_name << std::endl;

      // Output the bone's matrix.
      const mat4& m = motivators_[idx].Value();
      for (int row = 0; row < 3; ++row) {
        oss << "  (" << std::setw(7) << m(row, 0) << std::setw(7) << m(row, 1)
            << std::setw(7) << m(row, 2) << std::setw(7) << m(row, 3) << ')'
            << std::endl;
      }

      // Output the operations on this bone.
      const MatrixOpArray::OpVector& ops = current_anim_->Anim(idx).ops().ops();
      oss << "  ";
      for (size_t i = 0; i < ops.size(); ++i) {
        const float multiplier = RotateOp(ops[i].type) ? 180.0f / kPi : 1.0f;
        const float value =
            multiplier *
            motivators_[idx].ChildValue1f(static_cast<MotiveChildIndex>(i));
        oss << MatrixOpName(ops[i].type) << "=" << value;
        if (i < ops.size() - 1) {
          oss << ", ";
        } else {
          oss << std::endl;
        }
      }
      oss << std::endl;
    }
    return oss.str();
  }

 private:
  /// Traverse hierarchy, converting local transforms from `motivators_` into
  /// global transforms. The `parents` are layed out such that the parent
  /// always come before the child.
  // TODO OPT: optimize `parents` layout so that we can parallelize this call.
  void CalculateGlobalTransforms(AffineTransform* out) const {
    const BoneIndex* parents = defining_anim_->bone_parents();
    const int num_bones = NumBones();
    for (int i = 0; i < num_bones; ++i) {
      // TODO: Return an AffineTransform from the MatrixMotivator.
      const mat4& local_transform = motivators_[i].Value();
      const int parent_idx = parents[i];
      if (parent_idx == kInvalidBoneIdx) {
        out[i] = mat4::ToAffineTransform(local_transform);
      } else {
        assert(i > parent_idx);
        out[i] = mat4::ToAffineTransform(
            mat4::FromAffineTransform(out[parent_idx]) * local_transform);
      }
    }
  }

  MatrixMotivator4f* motivators_;
  AffineTransform* global_transforms_;
  const RigAnim* defining_anim_;
  const RigAnim* current_anim_;

  /// Time that the animation is expected to complete.
  MotiveTime end_time_;
};

// See comments on RigInit for details on this class.
class MotiveRigProcessor : public RigProcessor {
 public:
  MotiveRigProcessor() : time_(0) {}

  virtual ~MotiveRigProcessor() {
    RemoveIndices(0, NumIndices());
  }

  virtual void AdvanceFrame(MotiveTime delta_time) {
    Defragment();

    // Process the series of matrix operations for each index.
    const MotiveIndex num_indices = NumIndices();
    for (MotiveIndex index = 0; index < num_indices; ++index) {
      RigData& d = Data(index);
      d.UpdateGlobalTransforms();
    }

    // Update our global time. It shouldn't matter if this wraps
    // around, since we only calculate times relative to it.
    time_ += delta_time;
  }

  virtual void BlendToAnim(MotiveIndex index, const RigAnim& anim,
                           const motive::SplinePlayback& playback) {
    Data(index).BlendToAnim(anim, playback, time_);
  }

  virtual void SetPlaybackRate(MotiveIndex index, float playback_rate) {
    Data(index).SetPlaybackRate(playback_rate);
  }

  virtual MotivatorType Type() const { return RigInit::kType; }
  virtual int Priority() const { return 3; }

  virtual const AffineTransform* GlobalTransforms(MotiveIndex index) const {
    return Data(index).GlobalTransforms();
  }

  virtual MotiveTime TimeRemaining(MotiveIndex index) const {
    return Data(index).TimeRemaining();
  }

  virtual const RigAnim* DefiningAnim(MotiveIndex index) const {
    return Data(index).defining_anim();
  }

  const RigAnim* CurrentAnim(MotiveIndex index) const override {
    return Data(index).current_anim();
  }

  virtual std::string CsvHeaderForDebugging(MotiveIndex index) const {
    return Data(index).CsvHeaderForDebugging();
  }

  virtual std::string CsvValuesForDebugging(MotiveIndex index) const {
    return Data(index).CsvValuesForDebugging(time_);
  }

  virtual std::string LocalTransformsForDebugging(MotiveIndex index,
                                                  BoneIndex bone) const {
    return Data(index).LocalTransformsForDebugging(bone, time_);
  }

 protected:
  MotiveIndex NumIndices() const {
    return static_cast<MotiveIndex>(data_.size());
  }

  virtual void InitializeIndices(const MotivatorInit& init, MotiveIndex index,
                                 MotiveDimension dimensions,
                                 MotiveEngine* engine) {
    RemoveIndices(index, dimensions);
    auto rig_init = static_cast<const RigInit&>(init);
    for (MotiveIndex i = index; i < index + dimensions; ++i) {
      data_[i] = new RigData(rig_init, time_, engine);
    }
  }

  virtual void RemoveIndices(MotiveIndex index, MotiveDimension dimensions) {
    for (MotiveIndex i = index; i < index + dimensions; ++i) {
      if (data_[i] == nullptr) continue;
      delete data_[i];
      data_[i] = nullptr;
    }
  }

  virtual void MoveIndices(MotiveIndex old_index, MotiveIndex new_index,
                           MotiveDimension dimensions) {
    MotiveIndex old_i = old_index;
    MotiveIndex new_i = new_index;
    for (MotiveDimension i = 0; i < dimensions; ++i, ++new_i, ++old_i) {
      data_[new_i] = data_[old_i];
      data_[old_i] = nullptr;
    }
  }

  virtual void SetNumIndices(MotiveIndex num_indices) {
    // Ensure old items are deleted.
    const MotiveIndex old_num_indices = NumIndices();
    if (old_num_indices > num_indices) {
      RemoveIndices(num_indices, old_num_indices - num_indices);
    }

    // Initialize new items to nullptr.
    data_.resize(num_indices, nullptr);
  }

  const RigData& Data(MotiveIndex index) const {
    assert(ValidIndex(index));
    return *data_[index];
  }

  RigData& Data(MotiveIndex index) {
    assert(ValidIndex(index));
    return *data_[index];
  }

  std::vector<RigData*> data_;
  MotiveTime time_;
};

MOTIVE_INSTANCE(RigInit, MotiveRigProcessor);

}  // namespace motive
<commit_msg>Mark overridden methods with 'override'.<commit_after>// Copyright 2014 Google Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <iomanip>
#include <sstream>

#include "mathfu/constants.h"
#include "motive/anim.h"
#include "motive/engine.h"
#include "motive/init.h"
#include "motive/math/angle.h"
#include "motive/math/bulk_spline_evaluator.h"

using motive::Angle;
using motive::kPi;
using mathfu::AffineTransform;
using mathfu::mat4;
using mathfu::vec4;

namespace motive {

// static
static const MatrixOpArray kEmptyOps(0);

class RigData {
 public:
  explicit RigData(const RigInit& init, MotiveTime start_time,
                   MotiveEngine* engine)
      : motivators_(nullptr),
        global_transforms_(nullptr),
        defining_anim_(&init.defining_anim()),
        current_anim_(nullptr),
        end_time_(start_time) {
    const BoneIndex num_bones = defining_anim_->NumBones();

    // Visual Studio 2010 does not like std::vectors of mat4, since they are
    // a 16-byte aligned type. Use plain old arrays instead.
    motivators_ = new MatrixMotivator4f[num_bones];
    global_transforms_ = new AffineTransform[num_bones];

    // Initialize the motivators that drive the local transforms.
    for (BoneIndex i = 0; i < num_bones; ++i) {
      const MatrixOpArray& ops = defining_anim_->Anim(i).ops();
      motivators_[i].Initialize(MatrixInit(ops), engine);
    }

    // Initialize global transforms to default pose.
    // These will get overridden the first time AdvanceFrame() is called, but
    // we initialize them nicely anyway.
    UpdateGlobalTransforms();
  }

  ~RigData() {
    delete[] motivators_;
    motivators_ = nullptr;

    delete[] global_transforms_;
    global_transforms_ = nullptr;
  }

  void BlendToAnim(const RigAnim& anim, const motive::SplinePlayback& playback,
                   MotiveTime start_time) {
    end_time_ = start_time + anim.end_time();

    // When animation has only one bone, or mesh has only one bone,
    // we simply animate the root node only.
    const int anim_num_bones = anim.NumBones();
    const int defining_num_bones = NumBones();
    assert(defining_num_bones == 1 || anim_num_bones == 1 ||
           RigInit::MatchesHierarchy(anim, *defining_anim_));

    // Update the motivators to blend to our new values.
    for (BoneIndex i = 0; i < defining_num_bones; ++i) {
      const MatrixOpArray& ops =
          i >= anim_num_bones ? kEmptyOps : anim.Anim(i).ops();
      motivators_[i].BlendToOps(ops, playback);
    }

    // Remember the currently playing animation, for debugging purposes.
    current_anim_ = &anim;
  }

  const RigAnim* current_anim() const { return current_anim_; }

  void SetPlaybackRate(float playback_rate) {
    // Update the motivators to have the new playback rate.
    // TODO: Do this in bulk.
    const int defining_num_bones = NumBones();
    for (BoneIndex i = 0; i < defining_num_bones; ++i) {
      motivators_[i].SetPlaybackRate(playback_rate);
    }
  }

  MotiveTime TimeRemaining() const {
    if (end_time_ == kMotiveTimeEndless) {
      return kMotiveTimeEndless;
    }
    MotiveTime time = 0;
    const int defining_num_bones = NumBones();
    for (BoneIndex i = 0; i < defining_num_bones; ++i) {
      time = std::max(time, motivators_[i].TimeRemaining());
    }
    return time;
  }

  void UpdateGlobalTransforms() {
    CalculateGlobalTransforms(global_transforms_);
  }

  const AffineTransform* GlobalTransforms() const { return global_transforms_; }

  BoneIndex NumBones() const { return defining_anim_->NumBones(); }

  MotiveTime end_time() const { return end_time_; }

  const RigAnim* defining_anim() const { return defining_anim_; }

  void ChildValuesForDebugging(std::vector<float>* values) const {
    values->resize(defining_anim_->NumOps());

    int k = 0;
    const int defining_num_bones = NumBones();
    for (BoneIndex i = 0; i < defining_num_bones; ++i) {
      const MotiveChildIndex num_children = motivators_[i].NumChildren();
      for (MotiveChildIndex j = 0; j < num_children; ++j) {
        (*values)[k++] = motivators_[i].ChildValue1f(j);
      }
    }
  }

  std::string CsvHeaderForDebugging() const {
    std::ostringstream oss;
    oss << "animation name,time," << defining_anim_->CsvHeaderForDebugging(0)
        << std::endl;
    oss << ",," << defining_anim_->CsvHeaderForDebugging(1) << std::endl;
    return oss.str();
  }

  std::string CsvValuesForDebugging(MotiveTime current_time) const {
    std::vector<float> values;
    ChildValuesForDebugging(&values);

    std::ostringstream oss;
    const MotiveTime anim_time =
        current_time - end_time_ + current_anim_->end_time();
    oss << current_anim_->anim_name() << ',' << anim_time << ',';

    int k = 0;
    const int defining_num_bones = NumBones();
    for (BoneIndex i = 0; i < defining_num_bones; ++i) {
      const MatrixOpArray::OpVector& ops = defining_anim_->Anim(i).ops().ops();
      for (size_t j = 0; j < ops.size(); ++j) {
        const float multiplier = RotateOp(ops[j].type) ? 180.0f / kPi : 1.0f;
        oss << multiplier * values[k] << ',';
        k++;
      }
    }

    return oss.str();
  }

  std::string LocalTransformsForDebugging(BoneIndex bone,
                                          MotiveTime time) const {
    const BoneIndex* bone_parents = defining_anim_->bone_parents();

    // Output four lines: one per row of matrix.
    std::ostringstream oss;
    oss.precision(2);
    oss << std::fixed << std::right;

    // Output header
    const MotiveTime time_until_end = end_time_ - time;
    const MotiveTime time_since_start =
        current_anim_->end_time() - time_until_end;
    oss << current_anim_->anim_name() << " at time " << time_since_start << " ("
        << (time_since_start * 24.0f / 1000.0f) << " @24fps)" << std::endl;
    for (BoneIndex idx = bone; idx != kInvalidBoneIdx;
         idx = bone_parents[idx]) {
      // Output the bone's name.
      const char* bone_name = defining_anim_->BoneName(idx);
      oss << bone_name << std::endl;

      // Output the bone's matrix.
      const mat4& m = motivators_[idx].Value();
      for (int row = 0; row < 3; ++row) {
        oss << "  (" << std::setw(7) << m(row, 0) << std::setw(7) << m(row, 1)
            << std::setw(7) << m(row, 2) << std::setw(7) << m(row, 3) << ')'
            << std::endl;
      }

      // Output the operations on this bone.
      const MatrixOpArray::OpVector& ops = current_anim_->Anim(idx).ops().ops();
      oss << "  ";
      for (size_t i = 0; i < ops.size(); ++i) {
        const float multiplier = RotateOp(ops[i].type) ? 180.0f / kPi : 1.0f;
        const float value =
            multiplier *
            motivators_[idx].ChildValue1f(static_cast<MotiveChildIndex>(i));
        oss << MatrixOpName(ops[i].type) << "=" << value;
        if (i < ops.size() - 1) {
          oss << ", ";
        } else {
          oss << std::endl;
        }
      }
      oss << std::endl;
    }
    return oss.str();
  }

 private:
  /// Traverse hierarchy, converting local transforms from `motivators_` into
  /// global transforms. The `parents` are layed out such that the parent
  /// always come before the child.
  // TODO OPT: optimize `parents` layout so that we can parallelize this call.
  void CalculateGlobalTransforms(AffineTransform* out) const {
    const BoneIndex* parents = defining_anim_->bone_parents();
    const int num_bones = NumBones();
    for (int i = 0; i < num_bones; ++i) {
      // TODO: Return an AffineTransform from the MatrixMotivator.
      const mat4& local_transform = motivators_[i].Value();
      const int parent_idx = parents[i];
      if (parent_idx == kInvalidBoneIdx) {
        out[i] = mat4::ToAffineTransform(local_transform);
      } else {
        assert(i > parent_idx);
        out[i] = mat4::ToAffineTransform(
            mat4::FromAffineTransform(out[parent_idx]) * local_transform);
      }
    }
  }

  MatrixMotivator4f* motivators_;
  AffineTransform* global_transforms_;
  const RigAnim* defining_anim_;
  const RigAnim* current_anim_;

  /// Time that the animation is expected to complete.
  MotiveTime end_time_;
};

// See comments on RigInit for details on this class.
class MotiveRigProcessor : public RigProcessor {
 public:
  MotiveRigProcessor() : time_(0) {}

  virtual ~MotiveRigProcessor() {
    RemoveIndices(0, NumIndices());
  }

  void AdvanceFrame(MotiveTime delta_time) override {
    Defragment();

    // Process the series of matrix operations for each index.
    const MotiveIndex num_indices = NumIndices();
    for (MotiveIndex index = 0; index < num_indices; ++index) {
      RigData& d = Data(index);
      d.UpdateGlobalTransforms();
    }

    // Update our global time. It shouldn't matter if this wraps
    // around, since we only calculate times relative to it.
    time_ += delta_time;
  }

  void BlendToAnim(MotiveIndex index, const RigAnim& anim,
                   const motive::SplinePlayback& playback) override {
    Data(index).BlendToAnim(anim, playback, time_);
  }

  void SetPlaybackRate(MotiveIndex index, float playback_rate) override {
    Data(index).SetPlaybackRate(playback_rate);
  }

  MotivatorType Type() const override { return RigInit::kType; }
  int Priority() const override { return 3; }

  const AffineTransform* GlobalTransforms(MotiveIndex index) const override {
    return Data(index).GlobalTransforms();
  }

  MotiveTime TimeRemaining(MotiveIndex index) const override {
    return Data(index).TimeRemaining();
  }

  const RigAnim* DefiningAnim(MotiveIndex index) const override {
    return Data(index).defining_anim();
  }

  const RigAnim* CurrentAnim(MotiveIndex index) const override {
    return Data(index).current_anim();
  }

  std::string CsvHeaderForDebugging(MotiveIndex index) const override {
    return Data(index).CsvHeaderForDebugging();
  }

  std::string CsvValuesForDebugging(MotiveIndex index) const override {
    return Data(index).CsvValuesForDebugging(time_);
  }

  std::string LocalTransformsForDebugging(MotiveIndex index,
                                          BoneIndex bone) const override {
    return Data(index).LocalTransformsForDebugging(bone, time_);
  }

 protected:
  MotiveIndex NumIndices() const {
    return static_cast<MotiveIndex>(data_.size());
  }

  void InitializeIndices(const MotivatorInit& init, MotiveIndex index,
                         MotiveDimension dimensions,
                         MotiveEngine* engine) override {
    RemoveIndices(index, dimensions);
    auto rig_init = static_cast<const RigInit&>(init);
    for (MotiveIndex i = index; i < index + dimensions; ++i) {
      data_[i] = new RigData(rig_init, time_, engine);
    }
  }

  void RemoveIndices(MotiveIndex index, MotiveDimension dimensions) override {
    for (MotiveIndex i = index; i < index + dimensions; ++i) {
      if (data_[i] == nullptr) continue;
      delete data_[i];
      data_[i] = nullptr;
    }
  }

  void MoveIndices(MotiveIndex old_index, MotiveIndex new_index,
                   MotiveDimension dimensions) override {
    MotiveIndex old_i = old_index;
    MotiveIndex new_i = new_index;
    for (MotiveDimension i = 0; i < dimensions; ++i, ++new_i, ++old_i) {
      data_[new_i] = data_[old_i];
      data_[old_i] = nullptr;
    }
  }

  void SetNumIndices(MotiveIndex num_indices) override {
    // Ensure old items are deleted.
    const MotiveIndex old_num_indices = NumIndices();
    if (old_num_indices > num_indices) {
      RemoveIndices(num_indices, old_num_indices - num_indices);
    }

    // Initialize new items to nullptr.
    data_.resize(num_indices, nullptr);
  }

  const RigData& Data(MotiveIndex index) const {
    assert(ValidIndex(index));
    return *data_[index];
  }

  RigData& Data(MotiveIndex index) {
    assert(ValidIndex(index));
    return *data_[index];
  }

  std::vector<RigData*> data_;
  MotiveTime time_;
};

MOTIVE_INSTANCE(RigInit, MotiveRigProcessor);

}  // namespace motive
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <descartes_moveit/moveit_state_adapter.h>
#include <descartes_planner/dense_planner.h>
#include <descartes_trajectory/axial_symmetric_pt.h>
#include <descartes_trajectory/cart_trajectory_pt.h>
#include <moveit/move_group_interface/move_group.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit_msgs/DisplayTrajectory.h>

typedef std::vector<descartes_core::TrajectoryPtPtr> TrajectoryVec;
typedef TrajectoryVec::const_iterator TrajectoryIter;

/**
 * Generates an completely defined (zero-tolerance) cartesian point from a pose
 */
descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Affine3d& pose);
/**
 * Generates a cartesian point with free rotation about the Z axis of the EFF frame
 */
descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose);

/**
 * Translates a descartes trajectory to a ROS joint trajectory
 */
trajectory_msgs::JointTrajectory
toROSJointTrajectory(const TrajectoryVec& trajectory, const descartes_core::RobotModel& model,
                     const std::vector<std::string>& joint_names, double time_delay);

/**
 * Sends a ROS trajectory to the robot controller
 */
bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory);

/**
 * Plans for them, appends in overall_robot_traj
 */
void addFreeSpaceSegment(std::vector<geometry_msgs::Pose> waypoints, moveit::planning_interface::MoveGroup& group, robot_trajectory::RobotTrajectory& overall_robot_traj);
//second argument to avoid global variables

/**
 * Plans for process path points, and appends them to overall_robot_traj
 * TODO make argument for planner generic => Use pathPlannerBase
 */
void addProcessSegment(TrajectoryVec points, descartes_planner::DensePlanner& planner, moveit::planning_interface::MoveGroup& group, descartes_core::RobotModelPtr model, robot_trajectory::RobotTrajectory& overall_robot_traj, ros::NodeHandle& nh);

int main(int argc, char** argv)
{
  // Initialize ROS
  ros::init(argc, argv, "descartes_tutorial");
  ros::NodeHandle nh;

  // Required for communication with moveit components
  ros::AsyncSpinner spinner (1);
  spinner.start();

  // Initialize a MoveIt! group
  moveit::planning_interface::MoveGroup group("manipulator");

  // Move to home
  group.setNamedTarget("home");
  group.move();

  // Move to target pose
  geometry_msgs::Pose target_pose;
  target_pose.position.x = 0.25;
  target_pose.position.y = 0.25;
  target_pose.position.z = 1.0;
  group.setPoseTarget(target_pose);
  group.move();

  // Create a Descartes robot model and initialize it
  descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter);

  // Name of description on parameter server. Typically just "robot_description".
  const std::string robot_description = "robot_description";

  // name of the kinematic group you defined when running MoveitSetupAssistant
  // TODO Fermi has an issue of hardcoded manipulator. Avoid in Hamilton
  const std::string group_name = "manipulator";

  // Name of frame in which you are expressing poses. Typically "world_frame" or "base_link".
  const std::string world_frame = "base_link";

  // tool center point frame (name of link associated with tool)
  const std::string tcp_frame = "tool0";

  if (!model->initialize(robot_description, group_name, world_frame, tcp_frame))
  {
    ROS_INFO("Could not initialize robot model");
    return -1;
  }

  descartes_planner::DensePlanner planner;
  planner.initialize(model);
  robot_trajectory::RobotTrajectory overall_robot_traj(group.getCurrentState()->getRobotModel(), "manipulator");

  // Generate some MoveIt | free space waypoints
  std::vector<geometry_msgs::Pose> waypoints;
  for(int i = 0; i < 8; ++i)
  {
    target_pose.position.x = 0.25;
    target_pose.position.y = 0.25;
    target_pose.position.z = 1.0 + 0.05*i;
    waypoints.push_back(target_pose);
  }
  addFreeSpaceSegment(waypoints, group, overall_robot_traj);

  // Generate some Descartes | semi-constrained waypoints
  TrajectoryVec points;
  for (unsigned int i = 0; i < 10; ++i)
  {
    Eigen::Affine3d pose;
    pose = Eigen::Translation3d(0.25, 0.25, 1.4 - 0.05 * i);
    descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
    points.push_back(pt);
  }

  addProcessSegment(points, planner, group, model, overall_robot_traj, nh);

  moveit_msgs::RobotTrajectory combined;
  overall_robot_traj.getRobotTrajectoryMsg(combined);
  trajectory_msgs::JointTrajectory joints_combined = combined.joint_trajectory;

  //TOMIGHTDO http://docs.ros.org/hydro/api/moveit_core/html/classrobot__trajectory_1_1RobotTrajectory.html
  //This API could be improved by allowing for trajectory_msgs::JointTrajectory &trajectory argument in void getRobotTrajectoryMsg
  //Coz for setRobotTrajectory msg, you could pass trajectory_msgs::JointTrajectory or moveit_msgs::RobotTrajectory as done for Descartes above

  // Send the ROS trajectory to the robot for execution
  if (!executeTrajectory(joints_combined))
  {
    ROS_ERROR("Could not execute trajectory!");
    return -4;
  }
  // Wait till user kills the process (Control-C)
  ROS_INFO("Done!");
  return 0;
}

descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Affine3d& pose)
{
  using namespace descartes_core;
  using namespace descartes_trajectory;

  return TrajectoryPtPtr( new CartTrajectoryPt( TolerancedFrame(pose)) );
}

descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose)
{
  using namespace descartes_core;
  using namespace descartes_trajectory;
  return TrajectoryPtPtr( new AxialSymmetricPt(pose, M_PI/2.0-0.0001, AxialSymmetricPt::Z_AXIS) );
}

trajectory_msgs::JointTrajectory
toROSJointTrajectory(const TrajectoryVec& trajectory,
                     const descartes_core::RobotModel& model,
                     const std::vector<std::string>& joint_names,
                     double time_delay)
{
  // Fill out information about our trajectory
  trajectory_msgs::JointTrajectory result;
  result.header.stamp = ros::Time::now();
  result.header.frame_id = "world_frame";
  result.joint_names = joint_names;

  // For keeping track of time-so-far in the trajectory
  double time_offset = 0.0;
  // Loop through the trajectory
  for (TrajectoryIter it = trajectory.begin(); it != trajectory.end(); ++it)
  {
    // Find nominal joint solution at this point
    std::vector<double> joints;
    it->get()->getNominalJointPose(std::vector<double>(), model, joints);

    // Fill out a ROS trajectory point
    trajectory_msgs::JointTrajectoryPoint pt;
    pt.positions = joints;
    // velocity, acceleration, and effort are given dummy values
    // we'll let the controller figure them out
    pt.velocities.resize(joints.size(), 0.0);
    pt.accelerations.resize(joints.size(), 0.0);
    pt.effort.resize(joints.size(), 0.0);
    // set the time into the trajectory
    pt.time_from_start = ros::Duration(time_offset);
    // increment time
    time_offset += time_delay;

    result.points.push_back(pt);
  }

  return result;
}

bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory)
{
  // Create a Follow Joint Trajectory Action Client
  actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("joint_trajectory_action", true);
  if (!ac.waitForServer(ros::Duration(2.0)))
  {
    ROS_ERROR("Could not connect to action server");
    return false;
  }

  control_msgs::FollowJointTrajectoryGoal goal;
  goal.trajectory = trajectory;
  goal.goal_time_tolerance = ros::Duration(1.0);

  ac.sendGoal(goal);

  if (ac.waitForResult(goal.trajectory.points[goal.trajectory.points.size()-1].time_from_start + ros::Duration(5)))
  {
    ROS_INFO("Action server reported successful execution");
    return true;
  } else {
    ROS_WARN("Action server could not execute trajectory");
    return false;
  }
}

void addFreeSpaceSegment(std::vector<geometry_msgs::Pose> waypoints, moveit::planning_interface::MoveGroup& group, robot_trajectory::RobotTrajectory& overall_robot_traj)
{
  //MoveIt! planning
  moveit_msgs::RobotTrajectory moveit_robot_traj;
  group.setPlanningTime(10.0);
  double fraction = group.computeCartesianPath(waypoints,
                                               0.01,  // eef_step
                                               0,   // jump_threshold
                                               moveit_robot_traj);

  robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "manipulator");
  rt.setRobotTrajectoryMsg(*group.getCurrentState(), moveit_robot_traj);
  trajectory_processing::IterativeParabolicTimeParameterization iptp;
  bool success = iptp.computeTimeStamps(rt);
  ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");

  //Appending planned path
  // if(!overall_robot_traj.waypoints_.empty)
  overall_robot_traj.append(rt, rt.getWaypointDurationFromStart(rt.getWayPointCount()));
  ROS_INFO("Appended Free space segment");
}

void addProcessSegment(TrajectoryVec points, descartes_planner::DensePlanner& planner, moveit::planning_interface::MoveGroup& group, descartes_core::RobotModelPtr model, robot_trajectory::RobotTrajectory& overall_robot_traj, ros::NodeHandle& nh)
{
  if (!planner.planPath(points))
  {
    ROS_ERROR("Could not solve for a valid path");
  }

  TrajectoryVec result;
  if (!planner.getPath(result))
  {
    ROS_ERROR("Could not retrieve path");
  }

  // Translate the result into a type that ROS understands
  // Get Joint Names
  std::vector<std::string> names;
  nh.getParam("controller_joint_names", names);
  // Generate a ROS joint trajectory with the result path, robot model, given joint names,
  // a certain time delta between each trajectory point
  trajectory_msgs::JointTrajectory descartes_joint_solution = toROSJointTrajectory(result, *model, names, 1.0);

  //Append planned path
  robot_trajectory::RobotTrajectory descartes_robot_traj(group.getCurrentState()->getRobotModel(), "manipulator");
  descartes_robot_traj.setRobotTrajectoryMsg(*group.getCurrentState(), descartes_joint_solution);
  overall_robot_traj.append(descartes_robot_traj, descartes_robot_traj.getWaypointDurationFromStart(descartes_robot_traj.getWayPointCount()));
}
<commit_msg>minor changes<commit_after>#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <descartes_moveit/moveit_state_adapter.h>
#include <descartes_planner/dense_planner.h>
#include <descartes_trajectory/axial_symmetric_pt.h>
#include <descartes_trajectory/cart_trajectory_pt.h>
#include <moveit/move_group_interface/move_group.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit_msgs/DisplayTrajectory.h>

typedef std::vector<descartes_core::TrajectoryPtPtr> TrajectoryVec;
typedef TrajectoryVec::const_iterator TrajectoryIter;

/**
 * Generates an completely defined (zero-tolerance) cartesian point from a pose
 */
descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Affine3d& pose);
/**
 * Generates a cartesian point with free rotation about the Z axis of the EFF frame
 */
descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose);

/**
 * Translates a descartes trajectory to a ROS joint trajectory
 */
trajectory_msgs::JointTrajectory
toROSJointTrajectory(const TrajectoryVec& trajectory, const descartes_core::RobotModel& model,
                     const std::vector<std::string>& joint_names, double time_delay);

/**
 * Sends a ROS trajectory to the robot controller
 */
bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory);

/**
 * Plans for them, appends in overall_robot_traj
 */
void addFreeSpaceSegment(std::vector<geometry_msgs::Pose> waypoints, moveit::planning_interface::MoveGroup& group, robot_trajectory::RobotTrajectory& overall_robot_traj);
//second argument to avoid global variables

/**
 * Plans for process path points, and appends them to overall_robot_traj
 * TODO make argument for planner generic => Use pathPlannerBase
 */
void addProcessSegment(TrajectoryVec points, descartes_planner::DensePlanner& planner, moveit::planning_interface::MoveGroup& group, descartes_core::RobotModelPtr model, robot_trajectory::RobotTrajectory& overall_robot_traj, ros::NodeHandle& nh);

int main(int argc, char** argv)
{
  // Initialize ROS
  ros::init(argc, argv, "descartes_tutorial");
  ros::NodeHandle nh;

  // Required for communication with moveit components
  ros::AsyncSpinner spinner (1);
  spinner.start();

  // Initialize a MoveIt! group
  moveit::planning_interface::MoveGroup group("manipulator");

  // Move to home
  group.setNamedTarget("home");
  group.move();

  // Move to target pose
  geometry_msgs::Pose target_pose;
  target_pose.position.x = 0.25;
  target_pose.position.y = 0.25;
  target_pose.position.z = 1.0;
  group.setPoseTarget(target_pose);
  group.move();

  // Create a Descartes robot model and initialize it
  descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter);

  // Name of description on parameter server. Typically just "robot_description".
  const std::string robot_description = "robot_description";

  // name of the kinematic group you defined when running MoveitSetupAssistant
  // TODO Fermi has an issue of hardcoded manipulator. Avoid in Hamilton
  const std::string group_name = "manipulator";

  // Name of frame in which you are expressing poses. Typically "world_frame" or "base_link".
  const std::string world_frame = "base_link";

  // tool center point frame (name of link associated with tool)
  const std::string tcp_frame = "tool0";

  if (!model->initialize(robot_description, group_name, world_frame, tcp_frame))
  {
    ROS_INFO("Could not initialize robot model");
    return -1;
  }

  descartes_planner::DensePlanner planner;
  planner.initialize(model);
  robot_trajectory::RobotTrajectory overall_robot_traj(group.getCurrentState()->getRobotModel(), "manipulator");

  // Generate some MoveIt | free space waypoints
  std::vector<geometry_msgs::Pose> waypoints;
  for(int i = 0; i < 8; ++i)
  {
    target_pose.position.z = 1.0 + 0.05*i;
    waypoints.push_back(target_pose);
  }
  addFreeSpaceSegment(waypoints, group, overall_robot_traj);

  // Generate some Descartes | semi-constrained waypoints
  TrajectoryVec points;
  for (unsigned int i = 0; i < 10; ++i)
  {
    Eigen::Affine3d pose;
    pose = Eigen::Translation3d(0.25, 0.25, 1.4 - 0.05*i);
    descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
    points.push_back(pt);
  }
  addProcessSegment(points, planner, group, model, overall_robot_traj, nh);

  // Generate some MoveIt | free space waypoints
  std::vector<geometry_msgs::Pose> waypoints_2;
  for(int i = 0; i < 20; ++i)
  {
    target_pose.position.y -= 0.05;
    waypoints_2.push_back(target_pose);
  }
  addFreeSpaceSegment(waypoints_2, group, overall_robot_traj);

  TrajectoryVec points_2;
  for (unsigned int i = 0; i < 10; ++i)
  {
    Eigen::Affine3d pose;
    pose = Eigen::Translation3d(0.25 + 0.05*i, -0.75, 0.9);
    descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
    points_2.push_back(pt);
  }
  addProcessSegment(points_2, planner, group, model, overall_robot_traj, nh);

  moveit_msgs::RobotTrajectory combined;
  overall_robot_traj.getRobotTrajectoryMsg(combined);
  trajectory_msgs::JointTrajectory joints_combined = combined.joint_trajectory;

  //TOMIGHTDO http://docs.ros.org/hydro/api/moveit_core/html/classrobot__trajectory_1_1RobotTrajectory.html
  //This API could be improved by allowing for trajectory_msgs::JointTrajectory &trajectory argument in void getRobotTrajectoryMsg
  //Coz for setRobotTrajectory msg, you could pass trajectory_msgs::JointTrajectory or moveit_msgs::RobotTrajectory as done for Descartes above

  // Send the ROS trajectory to the robot for execution
  if (!executeTrajectory(joints_combined))
  {
    ROS_ERROR("Could not execute trajectory!");
    return -4;
  }
  // Wait till user kills the process (Control-C)
  ROS_INFO("Done!");
  return 0;
}

descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Affine3d& pose)
{
  using namespace descartes_core;
  using namespace descartes_trajectory;

  return TrajectoryPtPtr( new CartTrajectoryPt( TolerancedFrame(pose)) );
}

descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose)
{
  using namespace descartes_core;
  using namespace descartes_trajectory;
  return TrajectoryPtPtr( new AxialSymmetricPt(pose, M_PI/2.0-0.0001, AxialSymmetricPt::Z_AXIS) );
}

trajectory_msgs::JointTrajectory
toROSJointTrajectory(const TrajectoryVec& trajectory,
                     const descartes_core::RobotModel& model,
                     const std::vector<std::string>& joint_names,
                     double time_delay)
{
  // Fill out information about our trajectory
  trajectory_msgs::JointTrajectory result;
  result.header.stamp = ros::Time::now();
  result.header.frame_id = "world_frame";
  result.joint_names = joint_names;

  // For keeping track of time-so-far in the trajectory
  double time_offset = 0.0;
  // Loop through the trajectory
  for (TrajectoryIter it = trajectory.begin(); it != trajectory.end(); ++it)
  {
    // Find nominal joint solution at this point
    std::vector<double> joints;
    it->get()->getNominalJointPose(std::vector<double>(), model, joints);

    // Fill out a ROS trajectory point
    trajectory_msgs::JointTrajectoryPoint pt;
    pt.positions = joints;
    // velocity, acceleration, and effort are given dummy values
    // we'll let the controller figure them out
    pt.velocities.resize(joints.size(), 0.0);
    pt.accelerations.resize(joints.size(), 0.0);
    pt.effort.resize(joints.size(), 0.0);
    // set the time into the trajectory
    pt.time_from_start = ros::Duration(time_offset);
    // increment time
    time_offset += time_delay;

    result.points.push_back(pt);
  }

  return result;
}

bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory)
{
  // Create a Follow Joint Trajectory Action Client
  actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("joint_trajectory_action", true);
  if (!ac.waitForServer(ros::Duration(2.0)))
  {
    ROS_ERROR("Could not connect to action server");
    return false;
  }

  control_msgs::FollowJointTrajectoryGoal goal;
  goal.trajectory = trajectory;
  goal.goal_time_tolerance = ros::Duration(1.0);

  ac.sendGoal(goal);

  if (ac.waitForResult(goal.trajectory.points[goal.trajectory.points.size()-1].time_from_start + ros::Duration(5)))
  {
    ROS_INFO("Action server reported successful execution");
    return true;
  } else {
    ROS_WARN("Action server could not execute trajectory");
    return false;
  }
}

void addFreeSpaceSegment(std::vector<geometry_msgs::Pose> waypoints, moveit::planning_interface::MoveGroup& group, robot_trajectory::RobotTrajectory& overall_robot_traj)
{
  //MoveIt! planning
  moveit_msgs::RobotTrajectory moveit_robot_traj;
  group.setPlanningTime(10.0);
  double fraction = group.computeCartesianPath(waypoints,
                                               0.01,  // eef_step
                                               0,   // jump_threshold
                                               moveit_robot_traj);

  robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "manipulator");
  rt.setRobotTrajectoryMsg(*group.getCurrentState(), moveit_robot_traj);
  trajectory_processing::IterativeParabolicTimeParameterization iptp;
  bool success = iptp.computeTimeStamps(rt);
  ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");

  //Appending planned path
  // if(!overall_robot_traj.waypoints_.empty)
  overall_robot_traj.append(rt, rt.getWaypointDurationFromStart(rt.getWayPointCount()));
  ROS_INFO("Appended Free space segment");
}

void addProcessSegment(TrajectoryVec points, descartes_planner::DensePlanner& planner, moveit::planning_interface::MoveGroup& group, descartes_core::RobotModelPtr model, robot_trajectory::RobotTrajectory& overall_robot_traj, ros::NodeHandle& nh)
{
  if (!planner.planPath(points))
  {
    ROS_ERROR("Could not solve for a valid path");
  }

  TrajectoryVec result;
  if (!planner.getPath(result))
  {
    ROS_ERROR("Could not retrieve path");
  }

  // Translate the result into a type that ROS understands
  // Get Joint Names
  std::vector<std::string> names;
  nh.getParam("controller_joint_names", names);
  // Generate a ROS joint trajectory with the result path, robot model, given joint names,
  // a certain time delta between each trajectory point
  trajectory_msgs::JointTrajectory descartes_joint_solution = toROSJointTrajectory(result, *model, names, 1.0);

  //Append planned path
  robot_trajectory::RobotTrajectory descartes_robot_traj(group.getCurrentState()->getRobotModel(), "manipulator");
  descartes_robot_traj.setRobotTrajectoryMsg(*group.getCurrentState(), descartes_joint_solution);
  overall_robot_traj.append(descartes_robot_traj, descartes_robot_traj.getWaypointDurationFromStart(descartes_robot_traj.getWayPointCount()));
  ROS_INFO("Appended process segment");
}
<|endoftext|>


<commit_before>/**
 * Copyright 2014 Truphone
 */
#include "XmppTestCommand.h"

#include <QString>
#include <QObject>
#include <QXmppClient.h>
#include <QXmppMessage.h>

#include "Connection.h"
#include "XmppResourceStore.h"

namespace truphone
{
namespace test
{
namespace cascades
{
    const QString XMPPTestCommand::CMD_NAME = "xmppTest";

    XMPPTestCommand::XMPPTestCommand(Connection * const socket,
                                     QObject* parent)
        : Command(parent),
          client(socket)
    {
    }

    XMPPTestCommand::~XMPPTestCommand()
    {
    }

    bool XMPPTestCommand::executeCommand(QStringList * const arguments)
    {
        bool ret = false;
        if (arguments->length() < 2)
        {
            this->client->write(tr("ERROR: xmppTest <resource> <field> <optional: value>")
                                + "\r\n");
        }
        else
        {
            QXmppClient * const client =
                    XMPPResourceStore::instance()->getFromStore(arguments->first());
            arguments->removeFirst();
            if (client)
            {
                QSharedPointer<QXmppStanza> lastRecievedStanza
                        = XMPPResourceStore::instance()->getLastMessageReceived(
                            client);
                QSharedPointer<QXmppMessage> lastReceivedMessage(NULL);
                if (lastRecievedStanza)
                {
                    lastReceivedMessage
                            = lastRecievedStanza.dynamicCast<QXmppMessage>();
                }
                if (lastReceivedMessage)
                {
                    bool propertyOk = true;
                    const QString property = arguments->first();
                    arguments->removeFirst();
                    QVariant value;
                    // I tried to make this use reflection.
                    // It didn't work - might use reflection on function calls later
                    if (property == "body")
                    {
                        value = lastReceivedMessage->body();
                    }
                    else if (property == "attentionRequired")
                    {
                        value = lastReceivedMessage->isAttentionRequested();
                    }
                    else if (property == "mucInvitationJid")
                    {
                        value = lastReceivedMessage->mucInvitationJid();
                    }
                    else if (property == "mucInvitationPassword")
                    {
                        value = lastReceivedMessage->mucInvitationPassword();
                    }
                    else if (property == "mucInvitationReason")
                    {
                        value = lastReceivedMessage->mucInvitationReason();
                    }
                    else if (property == "receiptId")
                    {
                        value = lastReceivedMessage->receiptId();
                    }
                    else if (property == "stamp")
                    {
                        value = lastReceivedMessage->stamp();
                    }
                    else if (property == "state")
                    {
                        value = lastReceivedMessage->state();
                    }
                    else if (property == "subject")
                    {
                        value = lastReceivedMessage->subject();
                    }
                    else if (property == "thread")
                    {
                        value = lastReceivedMessage->thread();
                    }
                    else if (property == "type")
                    {
                        value = lastReceivedMessage->type();
                    }
                    else if (property == "hasForward")
                    {
                        value = lastReceivedMessage->hasForwarded();
                    }
                    else if (property == "forwardBody")
                    {
                        value = lastReceivedMessage->forwarded().body();
                    }
                    else if (property == "hasCarbon")
                    {
                        value = lastReceivedMessage->hasMessageCarbon();
                    }
                    else if (property == "carbonBody")
                    {
                        value = lastReceivedMessage->carbonMessage().body();
                    }
                    else if (property == "markable")
                    {
                        value = lastReceivedMessage->isMarkable();
                    }
                    else if (property == "marker")
                    {
                        value = lastReceivedMessage->marker();
                    }
                    else if (property == "markedId")
                    {
                        value = lastReceivedMessage->markedId();
                    }
                    else if (property == "markedThread")
                    {
                        value = lastReceivedMessage->markedThread();
                    }
                    else if (property == "isReplace")
                    {
                        value = lastReceivedMessage->isReplace();
                    }
                    else if (property == "replaceId")
                    {
                        value = lastReceivedMessage->replaceId();
                    }
                    else if (property == "from")
                    {
                        value = lastReceivedMessage->from();
                    }
                    else if (property == "to")
                    {
                        value = lastReceivedMessage->to();
                    }
                    else
                    {
                        propertyOk = false;
                    }
                    if (propertyOk)
                    {
                        QVariant expectedValue;
                        if (arguments->isEmpty())
                        {
                            ret = value.isNull() or not value.isValid();
                        }
                        else
                        {
                            const QString expected = arguments->join(" ");
                            ret = (value.toString() == expected);
                            expectedValue = expected;
                        }
                        if (not ret)
                        {
                            QString buffer = tr("ERROR: Value is {");
                            buffer += value.toString();
                            buffer += tr("} but expected {");
                            buffer += expectedValue.toString();
                            buffer += tr("}") + "\r\n";
                            this->client->write(buffer);
                        }
                    }
                    else
                    {
                        this->client->write(tr("ERROR: Unknown property") + "\r\n");
                    }
                }
                else
                {
                    this->client->write(tr("ERROR: Couldn't find the last received message")
                                        + "\r\n");
                }
            }
            else
            {
                this->client->write(tr("ERROR: Unknown resource") + "\r\n");
            }
        }
        return ret;
    }

    void XMPPTestCommand::showHelp()
    {
        this->client->write(tr("> xmppTest <resource> <field> <optional: value>") + "\r\n");
        this->client->write(tr("Test a field in the last received message") + "\r\n");
        this->client->write(tr("If <value> is missing it'll test for null/empty") + "\r\n");
        this->client->write(tr("Supported <value> options:") + "\r\n");
        this->client->write("\t" + tr("body") + "\r\n");
        this->client->write("\t" + tr("attentionRequired") + "\r\n");
        this->client->write("\t" + tr("mucInvitationJid") + "\r\n");
        this->client->write("\t" + tr("mucInvitationPassword") + "\r\n");
        this->client->write("\t" + tr("mucInvitationReason") + "\r\n");
        this->client->write("\t" + tr("receiptId") + "\r\n");
        this->client->write("\t" + tr("stamp") + "\r\n");
        this->client->write("\t" + tr("state") + "\r\n");
        this->client->write("\t" + tr("subject") + "\r\n");
        this->client->write("\t" + tr("thread") + "\r\n");
        this->client->write("\t" + tr("type") + "\r\n");
        this->client->write("\t" + tr("hasForward") + "\r\n");
        this->client->write("\t" + tr("fordwardBody") + "\r\n");
        this->client->write("\t" + tr("hasCarbon") + "\r\n");
        this->client->write("\t" + tr("carbonBody") + "\r\n");
        this->client->write("\t" + tr("markable") + "\r\n");
        this->client->write("\t" + tr("markedId") + "\r\n");
        this->client->write("\t" + tr("marker") + "\r\n");
        this->client->write("\t" + tr("markedThread") + "\r\n");
        this->client->write("\t" + tr("isReplace") + "\r\n");
        this->client->write("\t" + tr("replaceId") + "\r\n");
        this->client->write("\t" + tr("from") + "\r\n");
        this->client->write("\t" + tr("to") + "\r\n");

    }
}  // namespace cascades
}  // namespace test
}  // namespace truphone
<commit_msg>xmppTest supports checking of sent messages (for carbons).<commit_after>/**
 * Copyright 2014 Truphone
 */
#include "XmppTestCommand.h"

#include <QString>
#include <QObject>
#include <QXmppClient.h>
#include <QXmppMessage.h>

#include "Connection.h"
#include "XmppResourceStore.h"

namespace truphone
{
namespace test
{
namespace cascades
{
    const QString XMPPTestCommand::CMD_NAME = "xmppTest";

    XMPPTestCommand::XMPPTestCommand(Connection * const socket,
                                     QObject* parent)
        : Command(parent),
          client(socket)
    {
    }

    XMPPTestCommand::~XMPPTestCommand()
    {
    }

    bool XMPPTestCommand::executeCommand(QStringList * const arguments)
    {
        bool ret = false;
        if (arguments->length() < 2)
        {
            this->client->write(tr("ERROR: xmppTest <resource> <field> <optional: value>")
                                + "\r\n");
        }
        else
        {
            QXmppClient * const client =
                    XMPPResourceStore::instance()->getFromStore(arguments->first());
            arguments->removeFirst();
            if (client)
            {
                QSharedPointer<QXmppStanza> lastStanza(NULL);
                if (arguments->first() == "tx")
                {
                    arguments->removeFirst();
                    lastStanza = XMPPResourceStore::instance()->getLastMessageSent(client);
                }
                else
                {
                    lastStanza = XMPPResourceStore::instance()->getLastMessageReceived(client);
                }
                QSharedPointer<QXmppMessage> lastMessage(NULL);
                if (lastStanza)
                {
                    lastMessage
                            = lastStanza.dynamicCast<QXmppMessage>();
                }
                if (lastMessage)
                {
                    bool propertyOk = true;
                    const QString property = arguments->first();
                    arguments->removeFirst();
                    QVariant value;
                    // I tried to make this use reflection.
                    // It didn't work - might use reflection on function calls later
                    if (property == "body")
                    {
                        value = lastMessage->body();
                    }
                    else if (property == "attentionRequired")
                    {
                        value = lastMessage->isAttentionRequested();
                    }
                    else if (property == "mucInvitationJid")
                    {
                        value = lastMessage->mucInvitationJid();
                    }
                    else if (property == "mucInvitationPassword")
                    {
                        value = lastMessage->mucInvitationPassword();
                    }
                    else if (property == "mucInvitationReason")
                    {
                        value = lastMessage->mucInvitationReason();
                    }
                    else if (property == "receiptId")
                    {
                        value = lastMessage->receiptId();
                    }
                    else if (property == "stamp")
                    {
                        value = lastMessage->stamp();
                    }
                    else if (property == "state")
                    {
                        value = lastMessage->state();
                    }
                    else if (property == "subject")
                    {
                        value = lastMessage->subject();
                    }
                    else if (property == "thread")
                    {
                        value = lastMessage->thread();
                    }
                    else if (property == "type")
                    {
                        value = lastMessage->type();
                    }
                    else if (property == "hasForward")
                    {
                        value = lastMessage->hasForwarded();
                    }
                    else if (property == "forwardBody")
                    {
                        value = lastMessage->forwarded().body();
                    }
                    else if (property == "hasCarbon")
                    {
                        value = lastMessage->hasMessageCarbon();
                    }
                    else if (property == "carbonBody")
                    {
                        value = lastMessage->carbonMessage().body();
                    }
                    else if (property == "markable")
                    {
                        value = lastMessage->isMarkable();
                    }
                    else if (property == "marker")
                    {
                        value = lastMessage->marker();
                    }
                    else if (property == "markedId")
                    {
                        value = lastMessage->markedId();
                    }
                    else if (property == "markedThread")
                    {
                        value = lastMessage->markedThread();
                    }
                    else if (property == "isReplace")
                    {
                        value = lastMessage->isReplace();
                    }
                    else if (property == "replaceId")
                    {
                        value = lastMessage->replaceId();
                    }
                    else if (property == "from")
                    {
                        value = lastMessage->from();
                    }
                    else if (property == "to")
                    {
                        value = lastMessage->to();
                    }
                    else
                    {
                        propertyOk = false;
                    }
                    if (propertyOk)
                    {
                        QVariant expectedValue;
                        if (arguments->isEmpty())
                        {
                            ret = value.isNull() or not value.isValid();
                        }
                        else
                        {
                            const QString expected = arguments->join(" ");
                            ret = (value.toString() == expected);
                            expectedValue = expected;
                        }
                        if (not ret)
                        {
                            QString buffer = tr("ERROR: Value is {");
                            buffer += value.toString();
                            buffer += tr("} but expected {");
                            buffer += expectedValue.toString();
                            buffer += tr("}") + "\r\n";
                            this->client->write(buffer);
                        }
                    }
                    else
                    {
                        this->client->write(tr("ERROR: Unknown property") + "\r\n");
                    }
                }
                else
                {
                    this->client->write(tr("ERROR: Couldn't find the last received message")
                                        + "\r\n");
                }
            }
            else
            {
                this->client->write(tr("ERROR: Unknown resource") + "\r\n");
            }
        }
        return ret;
    }

    void XMPPTestCommand::showHelp()
    {
        this->client->write(tr("> xmppTest <resource> <opt: tx> <field> <optional: value>") + "\r\n");
        this->client->write(tr("Test a field in the last received message") + "\r\n");
        this->client->write(tr("If <tx> is specified, the last sent message is used."));
        this->client->write(tr("If <value> is missing it'll test for null/empty") + "\r\n");
        this->client->write(tr("Supported <value> options:") + "\r\n");
        this->client->write("\t" + tr("body") + "\r\n");
        this->client->write("\t" + tr("attentionRequired") + "\r\n");
        this->client->write("\t" + tr("mucInvitationJid") + "\r\n");
        this->client->write("\t" + tr("mucInvitationPassword") + "\r\n");
        this->client->write("\t" + tr("mucInvitationReason") + "\r\n");
        this->client->write("\t" + tr("receiptId") + "\r\n");
        this->client->write("\t" + tr("stamp") + "\r\n");
        this->client->write("\t" + tr("state") + "\r\n");
        this->client->write("\t" + tr("subject") + "\r\n");
        this->client->write("\t" + tr("thread") + "\r\n");
        this->client->write("\t" + tr("type") + "\r\n");
        this->client->write("\t" + tr("hasForward") + "\r\n");
        this->client->write("\t" + tr("fordwardBody") + "\r\n");
        this->client->write("\t" + tr("hasCarbon") + "\r\n");
        this->client->write("\t" + tr("carbonBody") + "\r\n");
        this->client->write("\t" + tr("markable") + "\r\n");
        this->client->write("\t" + tr("markedId") + "\r\n");
        this->client->write("\t" + tr("marker") + "\r\n");
        this->client->write("\t" + tr("markedThread") + "\r\n");
        this->client->write("\t" + tr("isReplace") + "\r\n");
        this->client->write("\t" + tr("replaceId") + "\r\n");
        this->client->write("\t" + tr("from") + "\r\n");
        this->client->write("\t" + tr("to") + "\r\n");

    }
}  // namespace cascades
}  // namespace test
}  // namespace truphone
<|endoftext|>


<commit_before>/*
 *
 *      Filename: test2.cpp
 *
 *        Author: Haibo Hao
 *        Email : haohaibo@ncic.ac.cn
 *   Description: ---
 *        Create: 2017-08-20 23:07:26
* Last Modified: 2017-08-20 23:59:05
 **/
#include <iostream>
#include <cstdio>

// 在C++代码编译过程中，如果程序员没有为类定义
// 析构函数，但是在销毁Y对象的时候又需要调用Y的
// 析构函数时，编译器会隐式的为该类生成一个析构
// 函数。该析构函数没有参数，包含一个空的函数体
// 即 ns::X::~X(){}
namespace ns {
    class X {
        private:
            int x;
    };
    class Y : public X {
        private:
            int y;
    };
}
int main(int argc, char* argv[])
{
    // 没有为基类ns::X和派生类ns::Y定义析构函数，
    // 当在主函数内delete基类指针x的时候，需要调
    // 用基类的析构函数。于是，编译器会隐式自动的
    // 为类X生成一个析构函数，从而可以成功的销毁x
    // 指向的派生类对象中的基类子对象(即int型成员
    // 变量x)

    // 这段代码存在内存泄露问题，当利用delete语句
    // 删除指向派生类对象的指针x时，系统调用的是基
    // 类的析构函数，而非派生类Y类的析构函数,因此
    // 编译器无法析构派生类的int型成员变量y
    ns::X* x = new ns::Y;
    delete x;
    return 0;
}

<commit_msg>add c++11 default<commit_after>/*
 *
 *      Filename: test2.cpp
 *
 *        Author: Haibo Hao
 *        Email : haohaibo@ncic.ac.cn
 *   Description: ---
 *        Create: 2017-08-20 23:07:26
* Last Modified: 2017-08-21 10:03:23
 **/
#include <iostream>
#include <cstdio>

// 在C++代码编译过程中，如果程序员没有为类定义
// 析构函数，但是在销毁Y对象的时候又需要调用Y的
// 析构函数时，编译器会隐式的为该类生成一个析构
// 函数。该析构函数没有参数，包含一个空的函数体
// 即 ns::X::~X(){}
namespace ns {
    class X {
        private:
            int x;
    };
    class Y : public X {
        private:
            int y;
    };

    class P {
        public:
            // 手动定义虚构函数
            virtual ~P(){};
        private:
            int p;
    };
    class Q : public P {
        private:
            int q;
    };

    class M {
        public:
            // 手动定义虚构函数
            virtual ~M()= default;
        private:
            int p;
    };
    class N : public M {
        private:
            int n;
    };
}
int main(int argc, char* argv[])
{
    // 没有为基类ns::X和派生类ns::Y定义析构函数，
    // 当在主函数内delete基类指针x的时候，需要调
    // 用基类的析构函数。于是，编译器会隐式自动的
    // 为类X生成一个析构函数，从而可以成功的销毁x
    // 指向的派生类对象中的基类子对象(即int型成员
    // 变量x)

    // 这段代码存在内存泄露问题，当利用delete语句
    // 删除指向派生类对象的指针x时，系统调用的是基
    // 类的析构函数，而非派生类Y类的析构函数,因此
    // 编译器无法析构派生类的int型成员变量y
    ns::X* x = new ns::Y;
    delete x;

    // 因此，一般情况下，我们需要将基类的析构函数
    // 定义为虚函数，当利用delete语句删除指向派生
    // 类对象的基类指针时，系统会调用相应的派生类
    // 的析构函数(实现多态性)，从而避免内存泄露。
    // 但是编译器隐式生成的析构函数都是非虚函数
    // 需要手动为基类P定义析构函数

    // 当利用delete语句删除指向派生类对象的基类指针
    // p，系统会调用相应的派生类Y的析构函数(有编译器
    // 隐式自动生成)以及基类P的析构函数，从而将派生类
    // 对象完整的销毁
    ns::P* p = new ns::Q;
    delete p;

    ns::M* m = new ns::N;
    delete m;

    return 0;
}

<|endoftext|>