feat(bag_time_manager_rviz_plugin): add bag time manager rviz plugin (t…

…ier4#1776)

* feat(bag_timemanager): add bag time manager rviz plugin

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix: fix change

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

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
+)

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.

common/bag_time_manager_rviz_plugin/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>bag_time_manager_rviz_plugin</name>
+  <version>0.0.1</version>
+  <description>Rviz plugin to publish and control the ros bag</description>
+  <maintainer email="taiki.tanaka@tier4.jp">Taiki Tanaka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>rosbag2_interfaces</depend>
+  <depend>rviz_common</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugins/plugin_description.xml"/>
+  </export>
+</package>

common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,9 @@
+<library path="bag_time_manager_rviz_plugin">
+
+  <class
+    type="rviz_plugins::BagTimeManagerPanel"
+    base_class_type="rviz_common::Panel">
+    <description>Panel that publishes a service to modify its speed.</description>
+  </class>
+
+</library>

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 <qt5/QtWidgets/QHBoxLayout>
+#include <qt5/QtWidgets/QLabel>
+#include <qt5/QtWidgets/QWidget>
+#include <rviz_common/display_context.hpp>
+
+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<Pause>("/rosbag2_player/pause", rmw_qos_profile_services_default);
+  client_resume_ =
+    raw_node_->create_client<Resume>("/rosbag2_player/resume", rmw_qos_profile_services_default);
+  client_set_rate_ =
+    raw_node_->create_client<SetRate>("/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<Resume::Request>();
+    client_resume_->async_send_request(
+      req, [this]([[maybe_unused]] rclcpp::Client<Resume>::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<Pause::Request>();
+    client_pause_->async_send_request(
+      req, [this]([[maybe_unused]] rclcpp::Client<Pause>::SharedFuture result) {});
+  }
+}
+
+void BagTimeManagerPanel::onApplyRateClicked()
+{
+  auto request = std::make_shared<SetRate::Request>();
+  request->rate = std::stod(rate_combo_->currentText().toStdString());
+  client_set_rate_->async_send_request(
+    request, [this, request](rclcpp::Client<SetRate>::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/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::BagTimeManagerPanel, rviz_common::Panel)

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 <qt5/QtWidgets/QComboBox>
+#include <qt5/QtWidgets/QLabel>
+#include <qt5/QtWidgets/QPushButton>
+#include <rclcpp/rclcpp.hpp>
+#include <rosbag2_interfaces/srv/pause.hpp>
+#include <rosbag2_interfaces/srv/resume.hpp>
+#include <rosbag2_interfaces/srv/set_rate.hpp>
+#include <rviz_common/panel.hpp>
+
+#include <memory>
+#include <string>
+
+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<Pause>::SharedPtr client_pause_;
+  rclcpp::Client<Resume>::SharedPtr client_resume_;
+  rclcpp::Client<SetRate>::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_