frames.cpp

Go to the documentation of this file.

#include <ram_qt_guis/frames.hpp>

namespace ram_qt_guis
{
Frames::Frames(QWidget* parent) :
        rviz::Panel(parent)
{
  setObjectName("Frame");
  setName(objectName());

  trajectory_frame_ = new Pose(QString::fromStdString("trajectory_frame"));
  trajectory_frame_->setText("Enter the robot trajectory frame:");
  start_pose_ = new Pose(QString::fromStdString("start_pose"));
  start_pose_->setText("Enter the start pose:");
  tool_ = new Pose(QString::fromStdString("tool"));
  tool_->setTranslationEnabled(false);
  tool_->setText("Enter the tool orientation:");

  QLabel* label(new QLabel("Display must be updated to see tool orientation changes when axis"
                           " are displayed on the trajectory."));
  label->setWordWrap(true);

  // Scroll area
  QVBoxLayout *scroll_widget_layout = new QVBoxLayout();
  QWidget *scroll_widget = new QWidget;
  scroll_widget->setLayout(scroll_widget_layout);
  QScrollArea *scroll_area = new QScrollArea;
  scroll_area->setWidget(scroll_widget);
  scroll_area->setWidgetResizable(true);
  scroll_area->setFrameShape(QFrame::NoFrame);

  QVBoxLayout* main_layout = new QVBoxLayout(this);
  main_layout->addWidget(scroll_area);
  scroll_widget_layout->addLayout(trajectory_frame_->getLayout());
  scroll_widget_layout->addStretch(1);
  scroll_widget_layout->addLayout(start_pose_->getLayout());
  scroll_widget_layout->addStretch(1);
  scroll_widget_layout->addLayout(tool_->getLayout());
  scroll_widget_layout->addWidget(label);
  scroll_widget_layout->addStretch(2);

  connect(this, SIGNAL(displayErrorMessageBox(const QString, const QString, const QString)), this,
          SLOT(displayErrorBoxHandler(const QString, const QString, const QString)));

  trajectory_frame_pub_ = nh_.advertise<geometry_msgs::Pose>("ram/trajectory_frame", 1);
  start_pose_pub_ = nh_.advertise<geometry_msgs::Pose>("ram/start_pose", 1);
  tool_pub_ = nh_.advertise<geometry_msgs::Pose>("ram/tool", 1);

  trajectory_frame_pose_.orientation.x = 0;
  trajectory_frame_pose_.orientation.y = 0;
  trajectory_frame_pose_.orientation.z = 0;
  trajectory_frame_pose_.orientation.w = 1;

  start_pose_pose_.orientation.x = 0;
  start_pose_pose_.orientation.y = 0;
  start_pose_pose_.orientation.z = 0;
  start_pose_pose_.orientation.w = 1;

  tool_pose_.orientation.x = 0;
  tool_pose_.orientation.y = 0;
  tool_pose_.orientation.z = 0;
  tool_pose_.orientation.w = 1;
}

Frames::~Frames()
{
}

void Frames::updateInternalParameters()
{
  tf::poseEigenToMsg(trajectory_frame_->getPose(), trajectory_frame_pose_);
  tf::poseEigenToMsg(start_pose_->getPose(), start_pose_pose_);
  tf::poseEigenToMsg(tool_->getPose(), tool_pose_);
}

void Frames::sendTrajectoryFrameInformation()
{
  updateInternalParameters();
  if (trajectory_frame_pub_.getNumSubscribers() == 0)
  {
    Q_EMIT displayErrorMessageBox(
                                  "Published message will be lost: ",
                                  QString::fromStdString(trajectory_frame_pub_.getTopic() + " has no subscriber"), "");
  }
  trajectory_frame_pub_.publish(trajectory_frame_pose_);
  ros::spinOnce();
}

void Frames::sendStartPoseInformation()
{
  updateInternalParameters();
  if (start_pose_pub_.getNumSubscribers() == 0)
  {
    Q_EMIT displayErrorMessageBox(
                                  "Published message will be lost: ",
                                  QString::fromStdString(start_pose_pub_.getTopic() + " has no subscriber"), "");
  }
  start_pose_pub_.publish(start_pose_pose_);
  ros::spinOnce();
}

void Frames::sendToolInformation()
{
  updateInternalParameters();
  if (tool_pub_.getNumSubscribers() == 0)
  {
    Q_EMIT displayErrorMessageBox(
                                  "Published message will be lost: ",
                                  QString::fromStdString(tool_pub_.getTopic() + " has no subscriber"), "");
  }
  tool_pub_.publish(tool_pose_);
  ros::spinOnce();
}

void Frames::load(const rviz::Config& config)
{
  trajectory_frame_->load(config);
  start_pose_->load(config);
  tool_->load(config);
  rviz::Panel::load(config);
  QtConcurrent::run(this, &Frames::sendLoadedInformation);
}

void Frames::sendLoadedInformation()
{
  Q_EMIT setEnabled(false);

  ros::Duration(1).sleep();
  while (nh_.ok())
  {
    if (trajectory_frame_pub_.getNumSubscribers() != 0 &&
        start_pose_pub_.getNumSubscribers() != 0 &&
        tool_pub_.getNumSubscribers() != 0)
      break;
    else
      ROS_WARN_STREAM_THROTTLE(1,
                               "RViz panel " << objectName().toStdString() + " did not connect to all the subscribers");
  }

  ROS_INFO_STREAM("RViz panel " << objectName().toStdString() << " subscribers connections have been made");

  sendTrajectoryFrameInformation();
  sendStartPoseInformation();
  sendToolInformation();

  connect(trajectory_frame_, SIGNAL(valueChanged()), this, SLOT(sendTrajectoryFrameInformation()));
  connect(start_pose_, SIGNAL(valueChanged()), this, SLOT(sendStartPoseInformation()));
  connect(tool_, SIGNAL(valueChanged()), this, SLOT(sendToolInformation()));

  // Connect with configChanged signal
  connect(trajectory_frame_, SIGNAL(valueChanged()), this, SIGNAL(configChanged()));
  connect(start_pose_, SIGNAL(valueChanged()), this, SIGNAL(configChanged()));
  connect(tool_, SIGNAL(valueChanged()), this, SIGNAL(configChanged()));

  Q_EMIT setEnabled(true);
}

void Frames::save(rviz::Config config) const
                  {
  trajectory_frame_->save(config);
  start_pose_->save(config);
  tool_->save(config);
  rviz::Panel::save(config);
}

void Frames::displayErrorBoxHandler(const QString title,
                                    const QString message,
                                    const QString info_msg)
{
  bool old_state(isEnabled());
  Q_EMIT setEnabled(false);
  QMessageBox msg_box;
  msg_box.setWindowTitle(title);
  msg_box.setText(message);
  msg_box.setInformativeText(info_msg);
  msg_box.setIcon(QMessageBox::Critical);
  msg_box.setStandardButtons(QMessageBox::Ok);
  msg_box.exec();
  Q_EMIT setEnabled(old_state);
}

}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(ram_qt_guis::Frames, rviz::Panel)