euler_property.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2016, Bielefeld University, CITEC
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  * Author: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
00030  */
00031 
00032 #include <QStringList>
00033 #include <rviz/properties/float_property.h>
00034 #include <rviz/properties/status_property.h>
00035 #include <angles/angles.h>
00036 #include <boost/format.hpp>
00037 #include <boost/assign/list_of.hpp>
00038 #include "euler_property.h"
00039 
00040 namespace rviz
00041 {
00042 
00043 EulerProperty::invalid_axes::invalid_axes(const std::string &msg) :
00044   std::invalid_argument(msg)
00045 {
00046 }
00047 
00048 EulerProperty::EulerProperty(Property* parent, const QString& name,
00049                              const Eigen::Quaterniond& value,
00050                              const char *changed_slot,
00051                              QObject* receiver)
00052   : Property(name, QVariant(),
00053              "Angles specified in degrees.\n"
00054              "Choose axes with spec like xyz, zxz, or rpy.\n"
00055              "Composition w.r.t. the static or rotating frame\n"
00056              "is selected by prefixing with 's' or 'r' (default).",
00057              parent, changed_slot, receiver)
00058   , quaternion_(value)
00059   , ignore_child_updates_(false)
00060   , angles_read_only_(false)
00061   , update_string_(true)
00062 {
00063   euler_[0] = new FloatProperty("", 0, "rotation angle about first axis", this);
00064   euler_[1] = new FloatProperty("", 0, "rotation angle about second axis", this);
00065   euler_[2] = new FloatProperty("", 0, "rotation angle about third axis", this);
00066   setEulerAxes("rpy");
00067 
00068   for (int i=0; i < 3; ++i) {
00069     connect(euler_[i], SIGNAL(aboutToChange()), this, SLOT(emitAboutToChange()));
00070     connect(euler_[i], SIGNAL(changed()), this, SLOT(updateFromChildren()));
00071   }
00072 }
00073 
00074 void EulerProperty::setQuaternion(const Eigen::Quaterniond& q)
00075 {
00076   if (quaternion_.isApprox(q)) return;
00077   updateAngles(q); // this will also emit changed signals (in setEulerAngles)
00078 }
00079 
00080 void EulerProperty::setEulerAngles(double euler[], bool normalize)
00081 {
00082   Eigen::Quaterniond q;
00083   if (fixed_)
00084     q = Eigen::AngleAxisd(euler[2], Eigen::Vector3d::Unit(axes_[2])) *
00085         Eigen::AngleAxisd(euler[1], Eigen::Vector3d::Unit(axes_[1])) *
00086         Eigen::AngleAxisd(euler[0], Eigen::Vector3d::Unit(axes_[0]));
00087   else
00088     q = Eigen::AngleAxisd(euler[0], Eigen::Vector3d::Unit(axes_[0])) *
00089         Eigen::AngleAxisd(euler[1], Eigen::Vector3d::Unit(axes_[1])) *
00090         Eigen::AngleAxisd(euler[2], Eigen::Vector3d::Unit(axes_[2]));
00091 
00092   if (normalize) setQuaternion(q);
00093   else {
00094     for (int i=0; i < 3; ++i) {
00095       float deg = angles::to_degrees(euler[i]);
00096       if (!Eigen::internal::isApprox(deg, euler_[i]->getFloat())) {
00097         update_string_ = true;
00098         if (!ignore_child_updates_)
00099           euler_[i]->setValue(deg);
00100       }
00101     }
00102 
00103     if (!quaternion_.isApprox(q)) {
00104       Q_EMIT aboutToChange();
00105       quaternion_ = q;
00106       Q_EMIT quaternionChanged(q);
00107     } else if (update_string_) {
00108       Q_EMIT aboutToChange();
00109     } else { // there is nothing to update at all
00110       return;
00111     }
00112     updateString();
00113     Q_EMIT changed();
00114   }
00115 }
00116 
00117 void EulerProperty::setEulerAngles(double e1, double e2, double e3, bool normalize)
00118 {
00119   double euler[3] = {e1,e2,e3};
00120   setEulerAngles(euler, normalize);
00121 }
00122 
00123 void EulerProperty::setEulerAxes(const QString &axes_spec)
00124 {
00125   static const std::vector<QString> xyzNames = boost::assign::list_of("x")("y")("z");
00126   static const std::vector<QString> rpyNames = boost::assign::list_of("roll")("pitch")("yaw");
00127   const std::vector<QString> *names = &xyzNames;
00128 
00129   if (axes_string_ == axes_spec) return;
00130   QString sAxes = axes_spec;
00131   if (sAxes == "rpy") {
00132     sAxes = "sxyz";
00133     names = &rpyNames;
00134   } else if (sAxes == "ypr") {
00135     sAxes = "rzyx";
00136     names = &rpyNames;
00137   }
00138 
00139   // static or rotated frame order?
00140   QString::iterator pc = sAxes.begin();
00141   bool fixed;
00142   if (*pc == 's') fixed = true;
00143   else if (*pc == 'r') fixed = false;
00144   else {fixed = false; --pc;}
00145   ++pc; // advance to first axis char
00146 
00147   // need to have 3 axes specs
00148   if (sAxes.end() - pc != 3)
00149     throw invalid_axes((boost::format("Invalid axes spec: %s. Expecting 3 chars from [xyz]")
00150                         % axes_spec.toStdString()).str());
00151 
00152   // parse axes specs into indexes
00153   uint axes[3];
00154   for (uint i=0; i < 3; ++i, ++pc) {
00155     int idx = pc->toLatin1() - 'x';
00156     if (idx < 0 || idx > 2)
00157       throw invalid_axes((boost::format("invalid axis char: %c (only xyz allowed)") % pc->unicode()).str());
00158     if (i > 0 && axes[i-1] == static_cast<uint>(idx))
00159       throw invalid_axes("consecutive axes need to be different");
00160     axes[i] = idx;
00161   }
00162 
00163   // everything OK: accept changes
00164   axes_string_ = axes_spec;
00165   fixed_ = fixed;
00166   for (int i=0; i < 3; ++i) {
00167     axes_[i] = axes[i];
00168     euler_[i]->setName((*names)[axes[i]]);
00169   }
00170 
00171   // finally compute euler angles matching the new axes
00172   update_string_ = true;
00173   updateAngles(quaternion_);
00174 }
00175 
00176 bool EulerProperty::setValue(const QVariant& value)
00177 {
00178   static const QString statusAxes ("Euler axes");
00179   static const QString statusAngles ("Euler angles");
00180 
00181   const QRegExp axesSpec("\\s*([a-z]+)\\s*:?");
00182   QString s = value.toString();
00183 
00184   // parse axes spec
00185   if (axesSpec.indexIn(s) != -1) {
00186     try {
00187       setEulerAxes(axesSpec.cap(1));
00188       Q_EMIT statusUpdate(StatusProperty::Ok, statusAxes, axes_string_);
00189     } catch (const invalid_axes &e) {
00190       Q_EMIT statusUpdate(StatusProperty::Warn, statusAxes, e.what());
00191       return false;
00192     }
00193     s = s.mid(axesSpec.matchedLength());
00194   }
00195 
00196   // in read-only mode only allow to change axes, but not angles
00197   if (angles_read_only_) {
00198     Q_EMIT statusUpdate(StatusProperty::Warn, statusAngles, "read-only");
00199     return true;
00200   }
00201 
00202   if (s.trimmed().isEmpty())
00203     return true; // allow change of axes only
00204 
00205   // parse angles
00206   QStringList strings = s.split(';');
00207   double euler[3];
00208   bool ok = true;
00209   for (int i=0; i < 3 && ok; ++i)
00210   {
00211     if (i < strings.size())
00212       euler[i] = angles::from_degrees(strings[i].toDouble(&ok));
00213     else // providing a single value will set all angles to this (1st) value
00214       euler[i] = euler[0];
00215   }
00216   if (!ok)
00217   {
00218     Q_EMIT statusUpdate(StatusProperty::Warn, statusAngles,
00219                         "failed to parse angle value");
00220     return false;
00221   }
00222   if (strings.size() != 3 && strings.size() != 1) {
00223     Q_EMIT statusUpdate(StatusProperty::Warn, statusAngles,
00224                         "expecting 3 semicolon-separated values");
00225     return false;
00226   }
00227 
00228   Q_EMIT statusUpdate(StatusProperty::Ok, statusAngles, "");
00229   setEulerAngles(euler, false);
00230   return true;
00231 }
00232 
00233 void EulerProperty::updateFromChildren()
00234 {
00235   if (ignore_child_updates_) return;
00236   double euler[3];
00237   for (int i = 0; i < 3; ++i)
00238     euler[i] = angles::from_degrees(euler_[i]->getValue().toFloat());
00239 
00240   ignore_child_updates_ = true;
00241   setEulerAngles(euler, false);
00242   ignore_child_updates_ = false;
00243 }
00244 
00245 void EulerProperty::emitAboutToChange()
00246 {
00247   if (ignore_child_updates_) return;
00248   Q_EMIT aboutToChange();
00249 }
00250 
00251 void EulerProperty::updateAngles(const Eigen::Quaterniond &q)
00252 {
00253   Eigen::Vector3d e;
00254   if (fixed_) {
00255     e = q.matrix().eulerAngles(axes_[2], axes_[1], axes_[0]);
00256     std::swap(e[0], e[2]);
00257   } else
00258     e = q.matrix().eulerAngles(axes_[0], axes_[1], axes_[2]);
00259   setEulerAngles(e.data(), false);
00260 }
00261 
00262 void EulerProperty::updateString()
00263 {
00264   QString s = QString("%1: %2; %3; %4")
00265       .arg(axes_string_)
00266       .arg(euler_[0]->getFloat(), 0, 'f', 1)
00267       .arg(euler_[1]->getFloat(), 0, 'f', 1)
00268       .arg(euler_[2]->getFloat(), 0, 'f', 1);
00269   value_ = s.replace(".0", "");
00270   update_string_ = false; // reset update flag
00271 }
00272 
00273 void EulerProperty::load(const Config& config)
00274 {
00275   QString axes;
00276   float euler[3];
00277   if (config.mapGetString("axes", &axes) &&
00278       config.mapGetFloat("e1", euler+0 ) &&
00279       config.mapGetFloat("e2", euler+1 ) &&
00280       config.mapGetFloat("e3", euler+2 ))
00281   {
00282     // Setting the value once is better than letting the Property class
00283     // load all parameters independently, which would result in 4 update calls
00284     setEulerAxes(axes);
00285     for (int i=0; i < 3; ++i)
00286       euler[i] = angles::from_degrees(euler[i]);
00287     setEulerAngles(euler[0], euler[1], euler[2], false);
00288   }
00289 }
00290 
00291 void EulerProperty::save(Config config) const
00292 {
00293   // Saving the child values explicitly avoids having Property::save()
00294   // save the summary string version of the property.
00295   config.mapSetValue("axes", axes_string_);
00296   config.mapSetValue("e1", euler_[0]->getValue());
00297   config.mapSetValue("e2", euler_[1]->getValue());
00298   config.mapSetValue("e3", euler_[2]->getValue());
00299 }
00300 
00301 void EulerProperty::setReadOnly(bool read_only)
00302 {
00303   // read-only mode should allow for changing axes, but not angles
00304   angles_read_only_ = read_only;
00305   // do not pass read-only to base class, but only to children
00306   for (int i=0; i < 3; ++i)
00307     euler_[i]->setReadOnly(read_only);
00308 }
00309 
00310 } // end namespace rviz


agni_tf_tools
Author(s): Robert Haschke
autogenerated on Sat Jun 8 2019 21:01:20