RViz plugin controlling real time factor value

Signed-off-by: Paweł Lech <[email protected]>

rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.14)
+project(real_time_factor_control_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Qt5 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/real_time_factor_slider.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
+  plugins
+)

rviz_plugins/real_time_factor_control_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>real_time_factor_control_rviz_plugin</name>
+  <version>0.1.0</version>
+  <description>Slider controlling real time factor value.</description>
+  <maintainer email="[email protected]">Paweł Lech</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Paweł Lech</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>libqt5-core</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rviz_common</depend>
+  <depend>std_msgs</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>

rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,9 @@
+<library path="real_time_factor_control_rviz_plugin">
+  <class name="real_time_factor_control_rviz_plugin/RealTimeFactorSliderPanel"
+          type="real_time_factor_control_rviz_plugin::RealTimeFactorSliderPanel"
+          base_class_type="rviz_common::Panel">
+      <description>
+        Slider controlling real time factor value.
+      </description>
+  </class>
+</library>

rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp

@@ -0,0 +1,64 @@
+//
+// Copyright 2020 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 "./real_time_factor_slider.hpp"
+
+#include "QHBoxLayout"
+#include "pluginlib/class_list_macros.hpp"
+#include "rviz_common/display_context.hpp"
+
+namespace real_time_factor_control_rviz_plugin
+{
+RealTimeFactorSliderPanel::RealTimeFactorSliderPanel(QWidget * parent) : rviz_common::Panel(parent)
+{
+  value_label_ = new QLabel("x 1.00");
+  value_label_->setAlignment(Qt::AlignCenter);
+
+  slider_ = new QSlider(Qt::Horizontal);
+  slider_->setMinimum(1);
+  slider_->setMaximum(200);
+  slider_->setTickInterval(1);
+  slider_->setValue(100);
+
+  auto * layout = new QHBoxLayout();
+  layout->addWidget(value_label_);
+  layout->addWidget(slider_);
+
+  setLayout(layout);
+}
+
+void RealTimeFactorSliderPanel::onChangedRealTimeFactorValue(int real_time_factor_value)
+{
+  std_msgs::msg::Float64 msg;
+  msg.data = real_time_factor_value / 100.0;
+  update_real_time_factor_publisher->publish(msg);
+  value_label_->setText(QString("x ") + QString::number(msg.data, 'f', 2));
+}
+
+void RealTimeFactorSliderPanel::onInitialize()
+{
+  rclcpp::Node::SharedPtr raw_node =
+    this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+  update_real_time_factor_publisher =
+    raw_node->create_publisher<std_msgs::msg::Float64>("/real_time_factor", 1);
+
+  connect(slider_, SIGNAL(valueChanged(int)), SLOT(onChangedRealTimeFactorValue(int)));
+}
+
+}  // namespace real_time_factor_control_rviz_plugin
+
+PLUGINLIB_EXPORT_CLASS(
+  real_time_factor_control_rviz_plugin::RealTimeFactorSliderPanel, rviz_common::Panel)

rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp

@@ -0,0 +1,54 @@
+//
+// Copyright 2023 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 REAL_TIME_FACTOR_SLIDER_PANEL_HPP_
+#define REAL_TIME_FACTOR_SLIDER_PANEL_HPP_
+
+#include "QLabel"
+#include "QSlider"
+
+#ifndef Q_MOC_RUN
+
+#include "rclcpp/rclcpp.hpp"
+#include "rviz_common/panel.hpp"
+
+#endif
+
+#include "std_msgs/msg/float64.hpp"
+
+namespace real_time_factor_control_rviz_plugin
+{
+class RealTimeFactorSliderPanel : public rviz_common::Panel
+{
+  Q_OBJECT
+
+public:
+  explicit RealTimeFactorSliderPanel(QWidget * parent = nullptr);
+  void onInitialize() override;
+
+public Q_SLOTS:
+  void onChangedRealTimeFactorValue(int real_time_factor_value);
+
+protected:
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr update_real_time_factor_publisher;
+
+  QLabel * value_label_;
+  QSlider * slider_;
+};
+
+}  // end namespace real_time_factor_control_rviz_plugin
+
+#endif  // REAL_TIME_FACTOR_SLIDER_PANEL_HPP_

simulation/traffic_simulator/include/traffic_simulator/api/api.hpp

@@ -84,8 +84,10 @@ class API
     real_time_factor_subscriber = rclcpp::create_subscription<std_msgs::msg::Float64>(
       node, "/real_time_factor", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(),
       [this](const std_msgs::msg::Float64 & message) {
+        if (message.data < 0.001) {
+          return;
+        }
         clock_.setRealTimeFactor(message.data);
-
         simulation_api_schema::UpdateStepTimeRequest request;
         request.set_simulation_step_time(clock_.getStepTime());
         zeromq_client_.call(request);