feat: add rviz plugin to publish and control the simulated clock (#349)

* Add tier4_clock_rviz_plugin to publish&control the sim clock in rviz

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Add step control

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix precommit

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Update documentation

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix spellcheck

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Update plugin description and icon

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Rename package

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix bug with long duration jumps (high speed + low rate)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.5)
+project(tier4_simulated_clock_rviz_plugin)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+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/simulated_clock_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)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  icons
+  plugins
+)

common/tier4_simulated_clock_rviz_plugin/README.md

@@ -0,0 +1,27 @@
+# tier4_simulated_clock_rviz_plugin
+
+## Purpose
+
+This plugin allows publishing and controlling the simulated ROS time.
+
+## Output
+
+| Name     | Type                        | Description                |
+| -------- | --------------------------- | -------------------------- |
+| `/clock` | `rosgraph_msgs::msg::Clock` | the current simulated time |
+
+## HowToUse
+
+1. Start rviz and select panels/Add new panel.
+   ![select_panel](./images/select_panels.png)
+2. Select tier4_clock_rviz_plugin/SimulatedClock and press OK.
+   ![select_clock_plugin](./images/select_clock_plugin.png)
+3. Use the added panel to control how the simulated clock is published.
+   ![use_clock_plugin](./images/use_clock_plugin.png)
+
+   - Pause button: pause/resume the clock.
+   - Speed: speed of the clock relative to the system clock.
+   - Rate: publishing rate of the clock.
+   - Step button: advance the clock by the specified time step.
+   - Time step: value used to advance the clock when pressing the step button d).
+   - Time unit: time unit associated with the value from e).

common/tier4_simulated_clock_rviz_plugin/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+
+  <name>tier4_simulated_clock_rviz_plugin</name>
+  <version>0.0.1</version>
+  <description>Rviz plugin to publish and control the /clock topic</description>
+  <maintainer email="maxime.clement@tier4.jp">Maxime CLEMENT</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>rosgraph_msgs</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/tier4_simulated_clock_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,9 @@
+<library path="tier4_simulated_clock_rviz_plugin">
+
+  <class
+    type="rviz_plugins::SimulatedClockPanel"
+    base_class_type="rviz_common::Panel">
+    <description>Panel that publishes a simulated clock to the /clock topic and provides an interface to pause the clock and modify its speed.</description>
+  </class>
+
+</library>

common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.cpp

@@ -0,0 +1,153 @@
+//
+//  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 "simulated_clock_panel.hpp"
+
+#include <qt5/QtWidgets/QGridLayout>
+#include <qt5/QtWidgets/QHBoxLayout>
+#include <qt5/QtWidgets/QLabel>
+#include <qt5/QtWidgets/QWidget>
+#include <rclcpp/duration.hpp>
+#include <rviz_common/display_context.hpp>
+
+#include <chrono>
+#include <string>
+
+namespace rviz_plugins
+{
+SimulatedClockPanel::SimulatedClockPanel(QWidget * parent) : rviz_common::Panel(parent)
+{
+  pause_button_ = new QPushButton("Pause");
+  pause_button_->setToolTip("Freeze ROS time.");
+  pause_button_->setCheckable(true);
+
+  publishing_rate_input_ = new QSpinBox();
+  publishing_rate_input_->setRange(1, 1000);
+  publishing_rate_input_->setSingleStep(1);
+  publishing_rate_input_->setValue(100);
+  publishing_rate_input_->setSuffix("Hz");
+
+  clock_speed_input_ = new QDoubleSpinBox();
+  clock_speed_input_->setRange(0.0, 10.0);
+  clock_speed_input_->setSingleStep(0.1);
+  clock_speed_input_->setValue(1.0);
+  clock_speed_input_->setSuffix(" X real time");
+
+  step_button_ = new QPushButton("Step");
+  step_button_->setToolTip("Pause and steps the simulation clock");
+  step_time_input_ = new QSpinBox();
+  step_time_input_->setRange(1, 999);
+  step_time_input_->setValue(1);
+  step_unit_combo_ = new QComboBox();
+  step_unit_combo_->addItems({"s", "ms", "µs", "ns"});
+
+  auto * layout = new QGridLayout(this);
+  auto * step_layout = new QHBoxLayout();
+  auto * clock_layout = new QHBoxLayout();
+  auto * clock_box = new QWidget();
+  auto * step_box = new QWidget();
+  clock_box->setLayout(clock_layout);
+  step_box->setLayout(step_layout);
+  layout->addWidget(pause_button_, 0, 0);
+  layout->addWidget(step_button_, 1, 0);
+  clock_layout->addWidget(new QLabel("Speed:"));
+  clock_layout->addWidget(clock_speed_input_);
+  clock_layout->addWidget(new QLabel("Rate:"));
+  clock_layout->addWidget(publishing_rate_input_);
+  step_layout->addWidget(step_time_input_);
+  step_layout->addWidget(step_unit_combo_);
+  layout->addWidget(clock_box, 0, 1, 1, 2);
+  layout->addWidget(step_box, 1, 1, 1, 2);
+  layout->setContentsMargins(0, 0, 20, 0);
+  prev_published_time_ = std::chrono::system_clock::now();
+
+  connect(publishing_rate_input_, SIGNAL(valueChanged(int)), this, SLOT(onRateChanged(int)));
+  connect(step_button_, SIGNAL(clicked()), this, SLOT(onStepClicked()));
+}
+
+void SimulatedClockPanel::onInitialize()
+{
+  raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+
+  clock_pub_ = raw_node_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", rclcpp::QoS(1));
+  createWallTimer();
+}
+
+void SimulatedClockPanel::onRateChanged(int new_rate)
+{
+  (void)new_rate;
+  pub_timer_->cancel();
+  createWallTimer();
+}
+
+void SimulatedClockPanel::onStepClicked()
+{
+  using std::chrono::duration_cast, std::chrono::seconds, std::chrono::milliseconds,
+    std::chrono::microseconds, std::chrono::nanoseconds;
+  pause_button_->setChecked(true);
+  const auto step_time = step_time_input_->value();
+  const auto unit = step_unit_combo_->currentText();
+  nanoseconds step_duration_ns{};
+  if (unit == "s") {
+    step_duration_ns += duration_cast<nanoseconds>(seconds(step_time));
+  } else if (unit == "ms") {
+    step_duration_ns += duration_cast<nanoseconds>(milliseconds(step_time));
+  } else if (unit == "µs") {
+    step_duration_ns += duration_cast<nanoseconds>(microseconds(step_time));
+  } else if (unit == "ns") {
+    step_duration_ns += duration_cast<nanoseconds>(nanoseconds(step_time));
+  }
+  addTimeToClock(step_duration_ns);
+}
+
+void SimulatedClockPanel::createWallTimer()
+{
+  // convert rate from Hz to milliseconds
+  const auto period =
+    std::chrono::milliseconds(static_cast<int64_t>(1e3 / publishing_rate_input_->value()));
+  pub_timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); });
+}
+
+void SimulatedClockPanel::onTimer()
+{
+  if (!pause_button_->isChecked()) {
+    const auto duration_since_prev_clock = std::chrono::system_clock::now() - prev_published_time_;
+    const auto speed_adjusted_duration = duration_since_prev_clock * clock_speed_input_->value();
+    addTimeToClock(std::chrono::duration_cast<std::chrono::nanoseconds>(speed_adjusted_duration));
+  }
+  clock_pub_->publish(clock_msg_);
+  prev_published_time_ = std::chrono::system_clock::now();
+}
+
+void SimulatedClockPanel::addTimeToClock(std::chrono::nanoseconds time_to_add_ns)
+{
+  constexpr auto one_sec = std::chrono::seconds(1);
+  constexpr auto one_sec_ns = std::chrono::nanoseconds(one_sec);
+  while (time_to_add_ns >= one_sec) {
+    time_to_add_ns -= one_sec;
+    clock_msg_.clock.sec += 1;
+  }
+  clock_msg_.clock.nanosec += time_to_add_ns.count();
+  if (clock_msg_.clock.nanosec >= one_sec_ns.count()) {
+    clock_msg_.clock.sec += 1;
+    clock_msg_.clock.nanosec = clock_msg_.clock.nanosec - one_sec_ns.count();
+  }
+}
+
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::SimulatedClockPanel, rviz_common::Panel)

common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.hpp

@@ -0,0 +1,76 @@
+//
+//  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 AUTOWARE_STATE_PANEL_HPP_
+#define AUTOWARE_STATE_PANEL_HPP_
+
+#include <qt5/QtWidgets/QComboBox>
+#include <qt5/QtWidgets/QDoubleSpinBox>
+#include <qt5/QtWidgets/QPushButton>
+#include <qt5/QtWidgets/QSpinBox>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/timer.hpp>
+#include <rviz_common/panel.hpp>
+
+#include <rosgraph_msgs/msg/clock.hpp>
+
+#include <memory>
+
+namespace rviz_plugins
+{
+class SimulatedClockPanel : public rviz_common::Panel
+{
+  Q_OBJECT
+
+public:
+  explicit SimulatedClockPanel(QWidget * parent = nullptr);
+  void onInitialize() override;
+
+protected Q_SLOTS:
+  /// @brief callback for when the publishing rate is changed
+  void onRateChanged(int new_rate);
+  /// @brief callback for when the step button is clicked
+  void onStepClicked();
+
+protected:
+  /// @brief creates ROS wall timer to periodically call onTimer()
+  void createWallTimer();
+  void onTimer();
+  /// @brief add some time to the clock
+  /// @input ns time to add in nanoseconds
+  void addTimeToClock(std::chrono::nanoseconds ns);
+
+  // ROS
+  rclcpp::Node::SharedPtr raw_node_;
+  rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_pub_;
+  rclcpp::TimerBase::SharedPtr pub_timer_;
+
+  // GUI
+  QPushButton * pause_button_;
+  QPushButton * step_button_;
+  QSpinBox * publishing_rate_input_;
+  QDoubleSpinBox * clock_speed_input_;
+  QSpinBox * step_time_input_;
+  QComboBox * step_unit_combo_;
+
+  // Clocks
+  std::chrono::time_point<std::chrono::system_clock> prev_published_time_;
+  rosgraph_msgs::msg::Clock clock_msg_;
+};
+
+}  // namespace rviz_plugins
+
+#endif  // AUTOWARE_STATE_PANEL_HPP_