time_panel.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * 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.
 */

#include <QLabel>
#include <QLineEdit>
#include <QPushButton>
#include <QHBoxLayout>
#include <QButtonGroup>
#include <QCheckBox>
#include <QSlider>
#include <QComboBox>

#include "visualization_manager.h"
#include "frame_manager.h"

#include "display_group.h"

#include "time_panel.h"

namespace rviz
{
TimePanel::TimePanel(QWidget* parent) : Panel(parent)
{
  wall_time_label_ = makeTimeLabel();
  wall_elapsed_label_ = makeTimeLabel();
  ros_time_label_ = makeTimeLabel();
  ros_elapsed_label_ = makeTimeLabel();

  experimental_cb_ = new QCheckBox("Experimental");
  experimental_cb_->setSizePolicy(QSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum));

  pause_button_ = new QPushButton("Pause");
  pause_button_->setToolTip("Freeze ROS time.");
  pause_button_->setCheckable(true);

  sync_mode_selector_ = new QComboBox(this);
  sync_mode_selector_->addItem("Off");
  sync_mode_selector_->addItem("Exact");
  sync_mode_selector_->addItem("Approximate");
  sync_mode_selector_->setSizeAdjustPolicy(QComboBox::AdjustToContents);
  sync_mode_selector_->setToolTip(
      "Allows you to synchronize the ROS time and Tf transforms to a given source.");

  // choose time sync signal
  sync_source_selector_ = new QComboBox(this);
  sync_source_selector_->setSizeAdjustPolicy(QComboBox::AdjustToContents);
  sync_source_selector_->setToolTip("Time source to use for synchronization.");

  experimental_widget_ = new QWidget(this);
  QHBoxLayout* experimental_layout = new QHBoxLayout(this);
  experimental_layout->addWidget(pause_button_);
  experimental_layout->addWidget(new QLabel("Synchronization:"));
  experimental_layout->addWidget(sync_mode_selector_);
  experimental_layout->addWidget(new QLabel("Source:"));
  experimental_layout->addWidget(sync_source_selector_);
  experimental_layout->addSpacing(20);
  experimental_layout->setContentsMargins(0, 0, 20, 0);
  experimental_widget_->setLayout(experimental_layout);

  old_widget_ = new QWidget(this);
  QHBoxLayout* old_layout = new QHBoxLayout(this);
  old_layout->addWidget(new QLabel("ROS Elapsed:"));
  old_layout->addWidget(ros_elapsed_label_);
  old_layout->addWidget(new QLabel("Wall Time:"));
  old_layout->addWidget(wall_time_label_);
  old_layout->addWidget(new QLabel("Wall Elapsed:"));
  old_layout->addWidget(wall_elapsed_label_);
  old_layout->setContentsMargins(0, 0, 20, 0);
  old_widget_->setLayout(old_layout);

  QHBoxLayout* layout = new QHBoxLayout(this);

  layout->addWidget(experimental_widget_);
  layout->addWidget(new QLabel("ROS Time:"));
  layout->addWidget(ros_time_label_);
  layout->addWidget(old_widget_);
  layout->addStretch(100);
  layout->addWidget(experimental_cb_);

  layout->addStretch();
  layout->setContentsMargins(11, 5, 11, 5);

  connect(experimental_cb_, SIGNAL(toggled(bool)), this, SLOT(experimentalToggled(bool)));
  connect(pause_button_, SIGNAL(toggled(bool)), this, SLOT(pauseToggled(bool)));
  connect(sync_mode_selector_, SIGNAL(activated(int)), this, SLOT(syncModeSelected(int)));
  connect(sync_source_selector_, SIGNAL(activated(int)), this, SLOT(syncSourceSelected(int)));
}

void TimePanel::onInitialize()
{
  connect(vis_manager_, SIGNAL(preUpdate()), this, SLOT(update()));

  DisplayGroup* display_group = vis_manager_->getRootDisplayGroup();
  onDisplayAdded(display_group);

  syncModeSelected(0);
  pauseToggled(false);
}

void TimePanel::load(const Config& config)
{
  Panel::load(config);
  int sync_mode;
  if (config.mapGetInt("SyncMode", &sync_mode))
  {
    sync_mode_selector_->setCurrentIndex(sync_mode);
    syncModeSelected(sync_mode);
  }
  config.mapGetString("SyncSource", &config_sync_source_);
  bool experimental = false;
  config.mapGetBool("Experimental", &experimental);
  experimental_cb_->setChecked(experimental);
  experimentalToggled(experimental);
}

void TimePanel::save(Config config) const
{
  Panel::save(config);
  config.mapSetValue("SyncMode", sync_mode_selector_->currentIndex());
  config.mapSetValue("SyncSource", sync_source_selector_->currentText());
  config.mapSetValue("Experimental", experimental_cb_->checkState() == Qt::Checked);
}

void TimePanel::onDisplayAdded(Display* display)
{
  DisplayGroup* display_group = qobject_cast<DisplayGroup*>(display);
  if (display_group)
  {
    connect(display_group, SIGNAL(displayAdded(rviz::Display*)), this,
            SLOT(onDisplayAdded(rviz::Display*)));
    connect(display_group, SIGNAL(displayRemoved(rviz::Display*)), this,
            SLOT(onDisplayRemoved(rviz::Display*)));

    for (int i = 0; i < display_group->numDisplays(); i++)
    {
      rviz::Display* display = display_group->getDisplayAt(i);
      onDisplayAdded(display);
    }
  }
  else
  {
    connect(display, SIGNAL(timeSignal(ros::Time)), this, SLOT(onTimeSignal(ros::Time)));
  }
}

void TimePanel::onDisplayRemoved(Display* display)
{
  QString name = display->getName();
  int index = sync_source_selector_->findData(QVariant((qulonglong)display));
  if (index >= 0)
  {
    sync_source_selector_->removeItem(index);
  }
}

void TimePanel::onTimeSignal(ros::Time time)
{
  Display* display = qobject_cast<Display*>(sender());
  if (!display)
    return;

  QString name = display->getName();
  int index = sync_source_selector_->findData(QVariant((qulonglong)display));

  // if we loaded the sync source name from the config, we need to
  // switch to it as soon as we get a signal
  if (index < 0 && name == config_sync_source_)
  {
    sync_source_selector_->addItem(name, QVariant((qulonglong)display));
    index = sync_source_selector_->findData(QVariant((qulonglong)display));
    sync_source_selector_->setCurrentIndex(index);
    config_sync_source_.clear();
  }

  if (index < 0)
  {
    sync_source_selector_->addItem(name, QVariant((qulonglong)display));
  }
  else
  {
    sync_source_selector_->setItemText(index, name);
    if (sync_source_selector_->currentIndex() == index)
    {
      vis_manager_->getFrameManager()->syncTime(time);
    }
  }
}

QLineEdit* TimePanel::makeTimeLabel()
{
  QLineEdit* label = new QLineEdit;
  label->setReadOnly(true);
  return label;
}

void TimePanel::fillTimeLabel(QLineEdit* label, double time)
{
  label->setText(QString::number(time, 'f', 2));
}

void TimePanel::update()
{
  fillTimeLabel(ros_time_label_, vis_manager_->getROSTime());
  fillTimeLabel(ros_elapsed_label_, vis_manager_->getROSTimeElapsed());
  fillTimeLabel(wall_time_label_, vis_manager_->getWallClock());
  fillTimeLabel(wall_elapsed_label_, vis_manager_->getWallClockElapsed());
}

void TimePanel::pauseToggled(bool checked)
{
  vis_manager_->getFrameManager()->setPause(checked);
}

void TimePanel::experimentalToggled(bool checked)
{
  old_widget_->setVisible(!checked);
  experimental_widget_->setVisible(checked);
  if (vis_manager_ && vis_manager_->getFrameManager())
  {
    if (!checked)
    {
      pauseToggled(false);
      syncModeSelected(0);
    }
    else
    {
      pauseToggled(pause_button_->isChecked());
      syncModeSelected(sync_mode_selector_->currentIndex());
    }
  }
}

void TimePanel::syncSourceSelected(int /*index*/)
{
  // clear whatever was loaded from the config
  config_sync_source_.clear();
  vis_manager_->notifyConfigChanged();
}

void TimePanel::syncModeSelected(int mode)
{
  vis_manager_->getFrameManager()->setSyncMode((FrameManager::SyncMode)mode);
  sync_source_selector_->setEnabled(mode != FrameManager::SyncOff);
  vis_manager_->notifyConfigChanged();
}

} // namespace rviz