|
| 1 | +// |
| 2 | +// Copyright 2022 Tier IV, Inc. All rights reserved. |
| 3 | +// |
| 4 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | +// you may not use this file except in compliance with the License. |
| 6 | +// You may obtain a copy of the License at |
| 7 | +// |
| 8 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | +// |
| 10 | +// Unless required by applicable law or agreed to in writing, software |
| 11 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | +// See the License for the specific language governing permissions and |
| 14 | +// limitations under the License. |
| 15 | +// |
| 16 | + |
| 17 | +#include "bag_time_manager_panel.hpp" |
| 18 | + |
| 19 | +#include <qt5/QtWidgets/QHBoxLayout> |
| 20 | +#include <qt5/QtWidgets/QLabel> |
| 21 | +#include <qt5/QtWidgets/QWidget> |
| 22 | +#include <rviz_common/display_context.hpp> |
| 23 | + |
| 24 | +namespace rviz_plugins |
| 25 | +{ |
| 26 | +BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent) |
| 27 | +{ |
| 28 | + // pause / resume |
| 29 | + { |
| 30 | + pause_button_ = new QPushButton("Pause"); |
| 31 | + pause_button_->setToolTip("Pause/Resume ROS time."); |
| 32 | + pause_button_->setStyleSheet("background-color: #00FF00;"); |
| 33 | + pause_button_->setCheckable(true); |
| 34 | + } |
| 35 | + |
| 36 | + // apply |
| 37 | + { |
| 38 | + apply_rate_button_ = new QPushButton("ApplyRate"); |
| 39 | + apply_rate_button_->setToolTip("control ROS time rate."); |
| 40 | + } |
| 41 | + |
| 42 | + // combo |
| 43 | + { |
| 44 | + rate_label_ = new QLabel("Rate:"); |
| 45 | + rate_label_->setAlignment(Qt::AlignCenter); |
| 46 | + rate_combo_ = new QComboBox(); |
| 47 | + rate_combo_->addItems({"0.01", "0.1", "0.5", "1.0", "2.0", "5.0", "10.0"}); |
| 48 | + rate_combo_->setCurrentText(QString("1.0")); |
| 49 | + time_label_ = new QLabel("X real time "); |
| 50 | + rate_label_->setAlignment(Qt::AlignCenter); |
| 51 | + } |
| 52 | + |
| 53 | + auto * layout = new QHBoxLayout(); |
| 54 | + layout->addWidget(pause_button_); |
| 55 | + layout->addWidget(apply_rate_button_); |
| 56 | + layout->addWidget(rate_label_); |
| 57 | + layout->addWidget(rate_combo_); |
| 58 | + layout->addWidget(time_label_); |
| 59 | + setLayout(layout); |
| 60 | + |
| 61 | + connect(pause_button_, SIGNAL(clicked()), this, SLOT(onPauseClicked())); |
| 62 | + connect(apply_rate_button_, SIGNAL(clicked()), this, SLOT(onApplyRateClicked())); |
| 63 | + connect(rate_combo_, SIGNAL(currentIndexChanged(int)), this, SLOT(onRateChanged())); |
| 64 | +} |
| 65 | + |
| 66 | +void BagTimeManagerPanel::onInitialize() |
| 67 | +{ |
| 68 | + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); |
| 69 | + client_pause_ = |
| 70 | + raw_node_-> create_client<Pause>( "/rosbag2_player/pause", rmw_qos_profile_services_default); |
| 71 | + client_resume_ = |
| 72 | + raw_node_->create_client<Resume>("/rosbag2_player/resume", rmw_qos_profile_services_default); |
| 73 | + client_set_rate_ = |
| 74 | + raw_node_->create_client<SetRate>("/rosbag2_player/set_rate", rmw_qos_profile_services_default); |
| 75 | +} |
| 76 | + |
| 77 | +void BagTimeManagerPanel::onPauseClicked() |
| 78 | +{ |
| 79 | + if (current_state_ == STATE::PAUSE) { |
| 80 | + // do resume |
| 81 | + current_state_ = STATE::RESUME; |
| 82 | + pause_button_->setText(QString::fromStdString("Resume")); |
| 83 | + // green |
| 84 | + pause_button_->setStyleSheet("background-color: #00FF00;"); |
| 85 | + auto req = std::make_shared<Resume::Request>(); |
| 86 | + client_resume_->async_send_request( |
| 87 | + req, [this]([[maybe_unused]] rclcpp::Client<Resume>::SharedFuture result) {}); |
| 88 | + } else { |
| 89 | + // do pause |
| 90 | + current_state_ = STATE::PAUSE; |
| 91 | + pause_button_->setText(QString::fromStdString("Pause")); |
| 92 | + // red |
| 93 | + pause_button_->setStyleSheet("background-color: #FF0000;"); |
| 94 | + auto req = std::make_shared<Pause::Request>(); |
| 95 | + client_pause_->async_send_request( |
| 96 | + req, [ this]([[maybe_unused]] rclcpp::Client<Pause>::SharedFuture result) {}); |
| 97 | + } |
| 98 | +} |
| 99 | + |
| 100 | +void BagTimeManagerPanel::onApplyRateClicked() |
| 101 | +{ |
| 102 | + auto request = std::make_shared<SetRate::Request>(); |
| 103 | + request->rate = std::stod(rate_combo_->currentText().toStdString()); |
| 104 | + client_set_rate_->async_send_request( |
| 105 | + request, [this, request](rclcpp::Client<SetRate>::SharedFuture result) { |
| 106 | + const auto & response = result.get(); |
| 107 | + if (response->success) { |
| 108 | + RCLCPP_INFO(raw_node_->get_logger(), "set ros bag rate %f x real time", request->rate); |
| 109 | + } else { |
| 110 | + RCLCPP_WARN(raw_node_->get_logger(), "service failed"); |
| 111 | + } |
| 112 | + }); |
| 113 | +} |
| 114 | +} // namespace rviz_plugins |
| 115 | + |
| 116 | +#include <pluginlib/class_list_macros.hpp> |
| 117 | +PLUGINLIB_EXPORT_CLASS(rviz_plugins::BagTimeManagerPanel, rviz_common::Panel) |
0 commit comments