rotation_property.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2016, Bielefeld University, CITEC
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
 */

#include "rotation_property.h"

using namespace rviz;

namespace agni_tf_tools {

RotationProperty::RotationProperty(Property* parent,
                                   const QString& name,
                                   const Eigen::Quaterniond& value,
                                   const char* changed_slot,
                                   QObject* receiver)
  : StringProperty(name,
                   "",
                   "Orientation specification using Euler angles or a quaternion.",
                   parent,
                   changed_slot,
                   receiver)
  , ignore_quaternion_property_updates_(false)
  , show_euler_string_(true) {
  euler_property_ = new EulerProperty(this, "Euler angles", value);
  quaternion_property_ =
      new rviz::QuaternionProperty("quaternion",
                                   Ogre::Quaternion(value.w(), value.x(), value.y(), value.z()),
                                   "order: x, y, z, w", this);
  connect(euler_property_, &EulerProperty::changed, this, &RotationProperty::updateFromEuler);
  connect(quaternion_property_, &QuaternionProperty::changed, this,
          &RotationProperty::updateFromQuaternion);
  // forward status signal from EulerProperty
  connect(euler_property_, &EulerProperty::statusUpdate, this, &RotationProperty::statusUpdate);
  // forward quaternion updates
  connect(euler_property_, &EulerProperty::quaternionChanged, this, &RotationProperty::quaternionChanged);
  updateString();
}

Eigen::Quaterniond RotationProperty::getQuaternion() const {
  return euler_property_->getQuaternion();
}

void RotationProperty::setQuaternion(const Eigen::Quaterniond& q) {
  Eigen::Quaterniond qn = q.normalized();
  if (getQuaternion().isApprox(qn))
    return;
  // EulerProperty is considered "master".
  // EulerProperty update will trigger QuaternionProperty update too
  euler_property_->setQuaternion(qn);
}

void RotationProperty::updateFromEuler() {
  const Eigen::Quaterniond q = euler_property_->getQuaternion();
  // do not update QuaternionProperty
  if (!ignore_quaternion_property_updates_) {
    quaternion_property_->setQuaternion(Ogre::Quaternion(q.w(), q.x(), q.y(), q.z()));
  }
  show_euler_string_ = true;
  updateString();
}

void RotationProperty::updateFromQuaternion() {
  // protect from infinite update loop
  if (ignore_quaternion_property_updates_)
    return;

  const Ogre::Quaternion& q = quaternion_property_->getQuaternion();
  Eigen::Quaternion<Ogre::Real> eigen_q(q.w, q.x, q.y, q.z);

  // only update if changes are within accuracy range
  if (eigen_q.isApprox(getQuaternion().cast<Ogre::Real>()))
    return;

  ignore_quaternion_property_updates_ = true;
  setQuaternion(eigen_q.cast<double>());
  ignore_quaternion_property_updates_ = false;

  show_euler_string_ = false;
  updateString();
}

void RotationProperty::setEulerAngles(double euler[], bool normalize) {
  euler_property_->setEulerAngles(euler, normalize);
}

void RotationProperty::setEulerAngles(double e1, double e2, double e3, bool normalize) {
  euler_property_->setEulerAngles(e1, e2, e3, normalize);
}

void RotationProperty::setEulerAxes(const QString& axes_spec) {
  euler_property_->setEulerAxes(axes_spec);
}

bool RotationProperty::setValue(const QVariant& value) {
  // forward parsing to either Quaternion- or EulerProperty
  const QRegExp quatSpec("\\s*(quat:)?([^;]+;){3}");
  QString s = value.toString();
  if (quatSpec.indexIn(s) != -1) {
    s = s.mid(quatSpec.cap(1).length());
    return quaternion_property_->setValue(s);
  }
  return euler_property_->setValue(value);
}

void RotationProperty::updateString() {
  QString euler = euler_property_->getValue().toString();
  QString quat = QString("quat: ") + quaternion_property_->getValue().toString();
  QString s = show_euler_string_ ? euler : quat;
  if (getString() != s) {
    Q_EMIT aboutToChange();
    value_ = s;
    Q_EMIT changed();
  }
}

void RotationProperty::load(const Config& config) {
  // save/load from EulerProperty. This handles both, quaternion and euler axes.
  euler_property_->load(config);
}

void RotationProperty::save(Config config) const {
  // save/load from EulerProperty. This handles both, quaternion and euler axes.
  euler_property_->save(config);
}

void RotationProperty::setReadOnly(bool read_only) {
  euler_property_->setReadOnly(read_only);
  quaternion_property_->setReadOnly(read_only);
}

} // namespace agni_tf_tools