pose_widget.cpp

Go to the documentation of this file.

#include <ram_qt_guis/modify_widgets/pose_widget.hpp>

namespace ram_qt_guis
{

Pose::Pose(const QString name,
           QWidget* parent) :
        deg2rad_(0.017453293),
        rad2deg_(57.2957795131),
        pose_(Eigen::Affine3d::Identity())
{
  setObjectName(name);

  layout_ = new QVBoxLayout(parent);

  label_pose_ = new QLabel(QString::fromStdString("Pose:"));
  layout_->addWidget(label_pose_);

  QLabel* label_pose_x = new QLabel("X");
  label_pose_x->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
  QLabel* label_pose_y = new QLabel("Y");
  QLabel* label_pose_z = new QLabel("Z");
  QLabel* label_pose_w = new QLabel("W");
  label_pose_w->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
  QLabel* label_pose_p = new QLabel("P");
  QLabel* label_pose_r = new QLabel("R");
  pose_x_ = new QDoubleSpinBox;
  pose_x_->setSuffix(" mm");
  pose_x_->setRange(-20000, 20000);
  pose_x_->setDecimals(3);
  pose_y_ = new QDoubleSpinBox;
  pose_y_->setSuffix(" mm");
  pose_y_->setRange(-20000, 20000);
  pose_y_->setDecimals(3);
  pose_z_ = new QDoubleSpinBox;
  pose_z_->setSuffix(" mm");
  pose_z_->setRange(-20000, 20000);
  pose_z_->setDecimals(3);
  pose_w_ = new QDoubleSpinBox;
  pose_w_->setSuffix(" deg");
  pose_w_->setRange(-180, 180);
  pose_w_->setDecimals(3);
  pose_p_ = new QDoubleSpinBox;
  pose_p_->setSuffix(" deg");
  pose_p_->setRange(-180, 180);
  pose_p_->setDecimals(3);
  pose_r_ = new QDoubleSpinBox;
  pose_r_->setSuffix(" deg");
  pose_r_->setRange(-180, 180);
  pose_r_->setDecimals(3);
  QGridLayout* pose_layout = new QGridLayout;
  layout_->addLayout(pose_layout);
  pose_layout->addWidget(label_pose_x, 0, 0);
  pose_layout->addWidget(pose_x_, 0, 1);
  pose_layout->addWidget(label_pose_y, 1, 0);
  pose_layout->addWidget(pose_y_, 1, 1);
  pose_layout->addWidget(label_pose_z, 2, 0);
  pose_layout->addWidget(pose_z_, 2, 1);
  pose_layout->addWidget(label_pose_w, 0, 2);
  pose_layout->addWidget(pose_w_, 0, 3);
  pose_layout->addWidget(label_pose_p, 1, 2);
  pose_layout->addWidget(pose_p_, 1, 3);
  pose_layout->addWidget(label_pose_r, 2, 2);
  pose_layout->addWidget(pose_r_, 2, 3);
  pose_x_->setLocale(QLocale(QLocale::English));
  pose_y_->setLocale(QLocale(QLocale::English));
  pose_z_->setLocale(QLocale(QLocale::English));
  pose_w_->setLocale(QLocale(QLocale::English));
  pose_p_->setLocale(QLocale(QLocale::English));
  pose_r_->setLocale(QLocale(QLocale::English));

  connect(pose_x_, SIGNAL(valueChanged(double)), this, SLOT(computePose()));
  connect(pose_y_, SIGNAL(valueChanged(double)), this, SLOT(computePose()));
  connect(pose_z_, SIGNAL(valueChanged(double)), this, SLOT(computePose()));
  connect(pose_w_, SIGNAL(valueChanged(double)), this, SLOT(computePose()));
  connect(pose_p_, SIGNAL(valueChanged(double)), this, SLOT(computePose()));
  connect(pose_r_, SIGNAL(valueChanged(double)), this, SLOT(computePose()));
}

void
Pose::save(rviz::Config config)
{
  config.mapSetValue(objectName() + "_x", pose_x_->value());
  config.mapSetValue(objectName() + "_y", pose_y_->value());
  config.mapSetValue(objectName() + "_z", pose_z_->value());
  config.mapSetValue(objectName() + "_w", pose_w_->value());
  config.mapSetValue(objectName() + "_p", pose_p_->value());
  config.mapSetValue(objectName() + "_r", pose_r_->value());
}

void
Pose::load(const rviz::Config &config)
{
  float tmp(0);
  if (config.mapGetFloat(objectName() + "_x", &tmp))
    pose_x_->setValue(tmp);
  if (config.mapGetFloat(objectName() + "_y", &tmp))
    pose_y_->setValue(tmp);
  if (config.mapGetFloat(objectName() + "_z", &tmp))
    pose_z_->setValue(tmp);
  if (config.mapGetFloat(objectName() + "_w", &tmp))
    pose_w_->setValue(tmp);
  if (config.mapGetFloat(objectName() + "_p", &tmp))
    pose_p_->setValue(tmp);
  if (config.mapGetFloat(objectName() + "_r", &tmp))
    pose_r_->setValue(tmp);
}

void
Pose::setTranslationEnabled(const bool enable)
{
  pose_x_->setEnabled(enable);
  pose_y_->setEnabled(enable);
  pose_z_->setEnabled(enable);
}

void
Pose::setText(const QString name)
{
  label_pose_->setText(name);
}

void
Pose::getPose(Eigen::Affine3d &pose)
{
  pose = pose_;
}

Eigen::Affine3d
Pose::getPose()
{
  return pose_;
}

void
Pose::resetPose()
{
  pose_ = Eigen::Affine3d::Identity();

  pose_x_->setValue(0);
  pose_y_->setValue(0);
  pose_z_->setValue(0);
  pose_w_->setValue(0);
  pose_p_->setValue(0);
  pose_r_->setValue(0);

  Q_EMIT valueChanged();
}

QVBoxLayout*
Pose::getLayout()
{
  return layout_;
}

void
Pose::computePose()
{
  pose_ = convertXYZWPRtoMatrix(pose_x_->value() / 1000.0,
                                pose_y_->value() / 1000.0,
                                pose_z_->value() / 1000.0,
                                deg2rad_ * pose_w_->value(),
                                deg2rad_ * pose_p_->value(),
                                deg2rad_ * pose_r_->value());

  Q_EMIT valueChanged();
}

Eigen::Affine3d Pose::convertXYZWPRtoMatrix(const double x,
                                            const double y,
                                            const double z,
                                            const double w,
                                            const double p,
                                            const double r)
{
  using Eigen::Affine3d;
  using Eigen::Matrix3d;
  using Eigen::Vector3d;
  using Eigen::AngleAxisd;

  Affine3d pose(Affine3d::Identity());
  pose.translation()[0] = x;
  pose.translation()[1] = y;
  pose.translation()[2] = z;
  Matrix3d rot;
  rot = AngleAxisd(w, Vector3d::UnitX())
      * AngleAxisd(p, Vector3d::UnitY())
      * AngleAxisd(r, Vector3d::UnitZ());
  pose.linear() = rot;

  return pose;
}

}