Merge pull request #1150 from tier4/feature/real-time-factor-control

Feature/real time factor control

rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.14)
+
+project(real_time_factor_control_rviz_plugin)
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(Qt5 REQUIRED Core Widgets)
+
+ament_auto_find_build_dependencies()
+
+set(QT_LIBRARIES Qt5::Widgets)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED src/real_time_factor_slider.cpp)
+
+target_compile_definitions(${PROJECT_NAME} PUBLIC QT_NO_KEYWORDS)
+
+target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src)
+
+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()

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</buildtool_depend>
+  <buildtool_depend>ament_cmake_auto</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,7 @@
+<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 <pluginlib/class_list_macros.hpp>
+#include <qt5/QtWidgets/QHBoxLayout>
+#include <real_time_factor_slider.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")),
+  slider_(new QSlider(Qt::Horizontal))
+{
+  value_label_->setAlignment(Qt::AlignCenter);
+
+  slider_->setMinimum(1);
+  slider_->setMaximum(200);
+  slider_->setTickInterval(1);
+  slider_->setValue(100);
+
+  auto layout = new QHBoxLayout(this);
+  layout->addWidget(value_label_);
+  layout->addWidget(slider_);
+
+  setLayout(layout);
+}
+
+auto RealTimeFactorSliderPanel::onChangedRealTimeFactorValue(int percentage) -> void
+{
+  std_msgs::msg::Float64 real_time_factor;
+  real_time_factor.data = percentage / 100.0;
+  real_time_factor_publisher->publish(real_time_factor);
+  value_label_->setText(QString("x ") + QString::number(real_time_factor.data, 'f', 2));
+}
+
+auto RealTimeFactorSliderPanel::onInitialize() -> void
+{
+  real_time_factor_publisher = getDisplayContext()
+                                 ->getRosNodeAbstraction()
+                                 .lock()
+                                 ->get_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,56 @@
+//
+// 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 <qt5/QtWidgets/QLabel>
+#include <qt5/QtWidgets/QSlider>
+#include <std_msgs/msg/float64.hpp>
+
+#ifndef Q_MOC_RUN
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/panel.hpp>
+#endif
+
+namespace real_time_factor_control_rviz_plugin
+{
+class RealTimeFactorSliderPanel : public rviz_common::Panel
+{
+  Q_OBJECT
+
+public:
+  explicit RealTimeFactorSliderPanel(QWidget * parent = nullptr);
+
+  auto onInitialize() -> void override;
+
+public Q_SLOTS:
+  /*
+     Declaring this function by trailing return type causes Qt AutoMoc
+     subprocess error.
+  */
+  void onChangedRealTimeFactorValue(int real_time_factor_value);
+
+protected:
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr real_time_factor_publisher;
+
+  QLabel * const value_label_;
+
+  QSlider * const slider_;
+};
+}  // namespace real_time_factor_control_rviz_plugin
+
+#endif  // REAL_TIME_FACTOR_SLIDER_PANEL_HPP_

simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp

@@ -96,6 +96,9 @@ class ScenarioSimulator : public rclcpp::Node
   auto updateFrame(const simulation_api_schema::UpdateFrameRequest &)
     -> simulation_api_schema::UpdateFrameResponse;
 
+  auto updateStepTime(const simulation_api_schema::UpdateStepTimeRequest &)
+    -> simulation_api_schema::UpdateStepTimeResponse;
+
   auto updateEntityStatus(const simulation_api_schema::UpdateEntityStatusRequest &)
     -> simulation_api_schema::UpdateEntityStatusResponse;
 

simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp

@@ -79,7 +79,7 @@ class EgoEntitySimulation
 
   explicit EgoEntitySimulation(
     const traffic_simulator_msgs::msg::VehicleParameters &, double,
-    const std::shared_ptr<hdmap_utils::HdMapUtils> &);
+    const std::shared_ptr<hdmap_utils::HdMapUtils> &, const rclcpp::Parameter & use_sim_time);
 
   auto update(double time, double step_time, bool npc_logic_started) -> void;
 

simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp

@@ -47,7 +47,8 @@ ScenarioSimulator::ScenarioSimulator(const rclcpp::NodeOptions & options)
     [this](auto &&... xs) { return followPolylineTrajectory(std::forward<decltype(xs)>(xs)...); },
     [this](auto &&... xs) {
       return attachPseudoTrafficLightDetector(std::forward<decltype(xs)>(xs)...);
-    })
+    },
+    [this](auto &&... xs) { return updateStepTime(std::forward<decltype(xs)>(xs)...); })
 {
 }
 
@@ -133,6 +134,15 @@ auto ScenarioSimulator::updateFrame(const simulation_api_schema::UpdateFrameRequ
   return res;
 }
 
+auto ScenarioSimulator::updateStepTime(const simulation_api_schema::UpdateStepTimeRequest & req)
+  -> simulation_api_schema::UpdateStepTimeResponse
+{
+  auto res = simulation_api_schema::UpdateStepTimeResponse();
+  step_time_ = req.simulation_step_time();
+  res.mutable_result()->set_success(true);
+  return res;
+}
+
 auto ScenarioSimulator::updateEntityStatus(
   const simulation_api_schema::UpdateEntityStatusRequest & req)
   -> simulation_api_schema::UpdateEntityStatusResponse
@@ -198,7 +208,8 @@ auto ScenarioSimulator::spawnVehicleEntity(
     traffic_simulator_msgs::msg::VehicleParameters parameters;
     simulation_interface::toMsg(req.parameters(), parameters);
     ego_entity_simulation_ = std::make_shared<vehicle_simulation::EgoEntitySimulation>(
-      parameters, step_time_, hdmap_utils_);
+      parameters, step_time_, hdmap_utils_,
+      get_parameter_or("use_sim_time", rclcpp::Parameter("use_sim_time", false)));
     traffic_simulator_msgs::msg::EntityStatus initial_status;
     initial_status.name = parameters.name;
     simulation_interface::toMsg(req.pose(), initial_status.pose);

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -34,12 +34,14 @@ static auto getParameter(const std::string & name, T value = {})
 
 EgoEntitySimulation::EgoEntitySimulation(
   const traffic_simulator_msgs::msg::VehicleParameters & parameters, double step_time,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils)
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils,
+  const rclcpp::Parameter & use_sim_time)
 : autoware(std::make_unique<concealer::AutowareUniverse>()),
   vehicle_model_type_(getVehicleModelType()),
   vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)),
   hdmap_utils_ptr_(hdmap_utils)
 {
+  autoware->set_parameter(use_sim_time);
 }
 
 auto toString(const VehicleModelType datum) -> std::string

simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp

@@ -49,6 +49,9 @@ class MultiClient
   auto call(const simulation_api_schema::UpdateFrameRequest &)
     -> simulation_api_schema::UpdateFrameResponse;
 
+  auto call(const simulation_api_schema::UpdateStepTimeRequest &)
+    -> simulation_api_schema::UpdateStepTimeResponse;
+
   auto call(const simulation_api_schema::SpawnVehicleEntityRequest &)
     -> simulation_api_schema::SpawnVehicleEntityResponse;
 

simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp

@@ -73,14 +73,15 @@ class MultiServer
   DEFINE_FUNCTION_TYPE(UpdateTrafficLights);
   DEFINE_FUNCTION_TYPE(FollowPolylineTrajectory);
   DEFINE_FUNCTION_TYPE(AttachPseudoTrafficLightDetector);
+  DEFINE_FUNCTION_TYPE(UpdateStepTime);
 
 #undef DEFINE_FUNCTION_TYPE
 
   std::tuple<
     Initialize, UpdateFrame, SpawnVehicleEntity, SpawnPedestrianEntity, SpawnMiscObjectEntity,
     DespawnEntity, UpdateEntityStatus, AttachLidarSensor, AttachDetectionSensor,
     AttachOccupancyGridSensor, UpdateTrafficLights, FollowPolylineTrajectory,
-    AttachPseudoTrafficLightDetector>
+    AttachPseudoTrafficLightDetector, UpdateStepTime>
     functions_;
 };
 }  // namespace zeromq

simulation/simulation_interface/proto/simulation_api_schema.proto

@@ -324,6 +324,20 @@ message FollowPolylineTrajectoryResponse {
   Result result = 1;
 }
 
+/**
+ * Requests updating simulation step time.
+ **/
+message UpdateStepTimeRequest {
+  double simulation_step_time = 1;
+}
+
+/**
+ * Response of updating simulation step time.
+ **/
+message UpdateStepTimeResponse {
+  Result result = 1; // Result of [UpdateStepTimeRequest](#UpdateStepTimeRequest)
+}
+
 /**
  * Universal message for Request
  **/
@@ -342,6 +356,7 @@ message SimulationRequest {
     UpdateTrafficLightsRequest update_traffic_lights = 11;
     FollowPolylineTrajectoryRequest follow_polyline_trajectory = 12;
     AttachPseudoTrafficLightDetectorRequest attach_pseudo_traffic_light_detector = 13;
+    UpdateStepTimeRequest update_step_time = 14;
   }
 }
 
@@ -364,5 +379,6 @@ message SimulationResponse {
     UpdateTrafficLightsResponse update_traffic_lights = 11;
     FollowPolylineTrajectoryResponse follow_polyline_trajectory = 12;
     AttachPseudoTrafficLightDetectorResponse attach_pseudo_traffic_light_detector = 13;
+    UpdateStepTimeResponse update_step_time = 14;
   }
 }

simulation/simulation_interface/src/zmq_multi_client.cpp

@@ -74,6 +74,18 @@ auto MultiClient::call(const simulation_api_schema::UpdateFrameRequest & request
   }
 }
 
+auto MultiClient::call(const simulation_api_schema::UpdateStepTimeRequest & request)
+  -> simulation_api_schema::UpdateStepTimeResponse
+{
+  if (is_running) {
+    simulation_api_schema::SimulationRequest sim_request;
+    *sim_request.mutable_update_step_time() = request;
+    return call(sim_request).update_step_time();
+  } else {
+    return {};
+  }
+}
+
 auto MultiClient::call(const simulation_api_schema::SpawnVehicleEntityRequest & request)
   -> simulation_api_schema::SpawnVehicleEntityResponse
 {

simulation/simulation_interface/src/zmq_multi_server.cpp

@@ -82,6 +82,10 @@ void MultiServer::poll()
           std::get<AttachPseudoTrafficLightDetector>(functions_)(
             proto.attach_pseudo_traffic_light_detector());
         break;
+      case simulation_api_schema::SimulationRequest::RequestCase::kUpdateStepTime:
+        *sim_response.mutable_update_step_time() =
+          std::get<UpdateStepTime>(functions_)(proto.update_step_time());
+        break;
       case simulation_api_schema::SimulationRequest::RequestCase::REQUEST_NOT_SET: {
         THROW_SIMULATION_ERROR("No case defined for oneof in SimulationRequest message");
       }

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

@@ -27,6 +27,7 @@
 #include <rosgraph_msgs/msg/clock.hpp>
 #include <simulation_interface/conversions.hpp>
 #include <simulation_interface/zmq_multi_client.hpp>
+#include <std_msgs/msg/float64.hpp>
 #include <stdexcept>
 #include <string>
 #include <traffic_simulator/api/configuration.hpp>
@@ -74,7 +75,21 @@ class API
       rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
     debug_marker_pub_(rclcpp::create_publisher<visualization_msgs::msg::MarkerArray>(
       node, "debug_marker", rclcpp::QoS(100), rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
-    clock_(std::forward<decltype(xs)>(xs)...),
+    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) {
+        /**
+         * @note Pausing the simulation by setting the realtime_factor_ value to 0 is not supported and causes the simulation crash.
+         * For that reason, before performing the action, it needs to be ensured that the incoming request data is a positive number.
+         */
+        if (message.data >= 0.001) {
+          clock_.realtime_factor = message.data;
+          simulation_api_schema::UpdateStepTimeRequest request;
+          request.set_simulation_step_time(clock_.getStepTime());
+          zeromq_client_.call(request);
+        }
+      })),
+    clock_(node->get_parameter("use_sim_time").as_bool(), std::forward<decltype(xs)>(xs)...),
     zeromq_client_(
       simulation_interface::protocol, configuration.simulator_host, getZMQSocketPort(*node))
   {
@@ -370,6 +385,8 @@ class API
 
   const rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
 
+  const rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr real_time_factor_subscriber;
+
   SimulationClock clock_;
 
   zeromq::MultiClient zeromq_client_;