diff --git a/common/bag_time_manager_rviz_plugin/CMakeLists.txt b/common/bag_time_manager_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..ccf9961fc55cc --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(bag_time_manager_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/bag_time_manager_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/bag_time_manager_rviz_plugin/README.md b/common/bag_time_manager_rviz_plugin/README.md new file mode 100644 index 0000000000000..2fad6b2204bb9 --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/README.md @@ -0,0 +1,26 @@ +# bag_time_manager_rviz_plugin + +## Purpose + +This plugin allows publishing and controlling the ros bag time. + +## Output + +tbd. + +## HowToUse + +1. Start rviz and select panels/Add new panel. + + ![select_panel](./images/select_panels.png) + +2. Select BagTimeManagerPanel and press OK. + + ![select_manager_plugin](./images/add_bag_time_manager_panel.png) + +3. See bag_time_manager_rviz_plugin/BagTimeManagerPanel is added. + + ![manager_plugin](./images/bag_time_manager_panel.png) + +- Pause/Resume: pause/resume the clock. +- ApplyRate: apply rate of the clock. diff --git a/common/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png b/common/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png new file mode 100644 index 0000000000000..9431c2d422363 Binary files /dev/null and b/common/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png differ diff --git a/common/bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png b/common/bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png new file mode 100644 index 0000000000000..8078c0ccbf34c Binary files /dev/null and b/common/bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png differ diff --git a/common/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png b/common/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png new file mode 100644 index 0000000000000..47ec05d5539c8 Binary files /dev/null and b/common/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png differ diff --git a/common/bag_time_manager_rviz_plugin/images/select_panels.png b/common/bag_time_manager_rviz_plugin/images/select_panels.png new file mode 100644 index 0000000000000..a691602c42c3c Binary files /dev/null and b/common/bag_time_manager_rviz_plugin/images/select_panels.png differ diff --git a/common/bag_time_manager_rviz_plugin/package.xml b/common/bag_time_manager_rviz_plugin/package.xml new file mode 100644 index 0000000000000..b12c24b728cde --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/package.xml @@ -0,0 +1,28 @@ + + + + bag_time_manager_rviz_plugin + 0.0.1 + Rviz plugin to publish and control the ros bag + Taiki Tanaka + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rosbag2_interfaces + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml b/common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..2a3bbf667b715 --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + Panel that publishes a service to modify its speed. + + + diff --git a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp new file mode 100644 index 0000000000000..dde3f10e9b554 --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp @@ -0,0 +1,117 @@ +// +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#include "bag_time_manager_panel.hpp" + +#include +#include +#include +#include + +namespace rviz_plugins +{ +BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + // pause / resume + { + pause_button_ = new QPushButton("Pause"); + pause_button_->setToolTip("Pause/Resume ROS time."); + pause_button_->setStyleSheet("background-color: #00FF00;"); + pause_button_->setCheckable(true); + } + + // apply + { + apply_rate_button_ = new QPushButton("ApplyRate"); + apply_rate_button_->setToolTip("control ROS time rate."); + } + + // combo + { + rate_label_ = new QLabel("Rate:"); + rate_label_->setAlignment(Qt::AlignCenter); + rate_combo_ = new QComboBox(); + rate_combo_->addItems({"0.01", "0.1", "0.5", "1.0", "2.0", "5.0", "10.0"}); + rate_combo_->setCurrentText(QString("1.0")); + time_label_ = new QLabel("X real time "); + rate_label_->setAlignment(Qt::AlignCenter); + } + + auto * layout = new QHBoxLayout(); + layout->addWidget(pause_button_); + layout->addWidget(apply_rate_button_); + layout->addWidget(rate_label_); + layout->addWidget(rate_combo_); + layout->addWidget(time_label_); + setLayout(layout); + + connect(pause_button_, SIGNAL(clicked()), this, SLOT(onPauseClicked())); + connect(apply_rate_button_, SIGNAL(clicked()), this, SLOT(onApplyRateClicked())); + connect(rate_combo_, SIGNAL(currentIndexChanged(int)), this, SLOT(onRateChanged())); +} + +void BagTimeManagerPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + client_pause_ = + raw_node_->create_client("/rosbag2_player/pause", rmw_qos_profile_services_default); + client_resume_ = + raw_node_->create_client("/rosbag2_player/resume", rmw_qos_profile_services_default); + client_set_rate_ = + raw_node_->create_client("/rosbag2_player/set_rate", rmw_qos_profile_services_default); +} + +void BagTimeManagerPanel::onPauseClicked() +{ + if (current_state_ == STATE::PAUSE) { + // do resume + current_state_ = STATE::RESUME; + pause_button_->setText(QString::fromStdString("Resume")); + // green + pause_button_->setStyleSheet("background-color: #00FF00;"); + auto req = std::make_shared(); + client_resume_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + } else { + // do pause + current_state_ = STATE::PAUSE; + pause_button_->setText(QString::fromStdString("Pause")); + // red + pause_button_->setStyleSheet("background-color: #FF0000;"); + auto req = std::make_shared(); + client_pause_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + } +} + +void BagTimeManagerPanel::onApplyRateClicked() +{ + auto request = std::make_shared(); + request->rate = std::stod(rate_combo_->currentText().toStdString()); + client_set_rate_->async_send_request( + request, [this, request](rclcpp::Client::SharedFuture result) { + const auto & response = result.get(); + if (response->success) { + RCLCPP_INFO(raw_node_->get_logger(), "set ros bag rate %f x real time", request->rate); + } else { + RCLCPP_WARN(raw_node_->get_logger(), "service failed"); + } + }); +} +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::BagTimeManagerPanel, rviz_common::Panel) diff --git a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp new file mode 100644 index 0000000000000..2cc4b98a6dabf --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp @@ -0,0 +1,72 @@ +// +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#ifndef BAG_TIME_MANAGER_PANEL_HPP_ +#define BAG_TIME_MANAGER_PANEL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace rviz_plugins +{ +using rosbag2_interfaces::srv::Pause; +using rosbag2_interfaces::srv::Resume; +using rosbag2_interfaces::srv::SetRate; +class BagTimeManagerPanel : public rviz_common::Panel +{ + Q_OBJECT +public: + explicit BagTimeManagerPanel(QWidget * parent = nullptr); + void onInitialize() override; + +protected Q_SLOTS: + /// @brief callback for when the publishing rate is changed + void onRateChanged() {} + /// @brief callback for when the step button is clicked + void onPauseClicked(); + void onApplyRateClicked(); + +protected: + // ROS + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Client::SharedPtr client_pause_; + rclcpp::Client::SharedPtr client_resume_; + rclcpp::Client::SharedPtr client_set_rate_; + + // GUI + QPushButton * pause_button_; + QPushButton * apply_rate_button_; + QLabel * rate_label_; + QLabel * time_label_; + QComboBox * rate_combo_; + +private: + enum STATE { PAUSE, RESUME }; + STATE current_state_{RESUME}; +}; + +} // namespace rviz_plugins + +#endif // BAG_TIME_MANAGER_PANEL_HPP_