From 17a9ec52ab8548fef31d2947686914e330786394 Mon Sep 17 00:00:00 2001 From: Piotr Zyskowski Date: Mon, 27 Nov 2023 15:41:09 +0100 Subject: [PATCH 01/12] realtime factor fix --- .../traffic_simulator/simulation_clock/simulation_clock.hpp | 4 ++-- .../src/simulation_clock/simulation_clock.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp index d6d5deaed5f..62a3a105c01 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp @@ -29,9 +29,9 @@ class SimulationClock : rclcpp::Clock auto getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock; - auto getCurrentScenarioTime() const { return (frame_ - frame_offset_) / frame_rate; } + auto getCurrentScenarioTime() const { return (frame_ - frame_offset_) / frame_rate * realtime_factor; } - auto getCurrentSimulationTime() const { return frame_ / frame_rate; } + auto getCurrentSimulationTime() const { return frame_ / frame_rate * realtime_factor; } auto getStepTime() const { return realtime_factor / frame_rate; } diff --git a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp index df996c0dff3..9cce02cb367 100644 --- a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp +++ b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp @@ -19,10 +19,10 @@ namespace traffic_simulator { SimulationClock::SimulationClock(double realtime_factor, double frame_rate) : rclcpp::Clock(RCL_ROS_TIME), - use_raw_clock(true), + use_raw_clock(false), realtime_factor(realtime_factor), frame_rate(frame_rate), - time_on_initialize(now()) + time_on_initialize(0, 0) { } From 7a71e2a5a23e5ad6e829c08a0b94b0f99759ce22 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Tue, 5 Dec 2023 10:02:09 +0100 Subject: [PATCH 02/12] Global real time factor set with launch argument fix MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- external/concealer/src/autoware.cpp | 2 +- .../scenario_test_runner/launch/scenario_test_runner.launch.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/external/concealer/src/autoware.cpp b/external/concealer/src/autoware.cpp index a209e4c8f84..2adc692db72 100644 --- a/external/concealer/src/autoware.cpp +++ b/external/concealer/src/autoware.cpp @@ -17,7 +17,7 @@ namespace concealer { Autoware::Autoware() -: rclcpp::Node("concealer", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), +: rclcpp::Node("concealer", "simulation", rclcpp::NodeOptions().use_global_arguments(true)), current_acceleration(geometry_msgs::msg::Accel()), current_twist(geometry_msgs::msg::Twist()), current_pose(geometry_msgs::msg::Pose()) diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 047b9515896..66255687dfc 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -171,7 +171,8 @@ def description(): namespace="simulation", output="screen", on_exit=ShutdownOnce(), - parameters=[{"port": port}]+make_vehicle_parameters(), + parameters=[{"port": port}]+make_vehicle_parameters()+[{"use_sim_time": True}], + # parameters=[{"port": port}]+make_vehicle_parameters(), condition=IfCondition(launch_simple_sensor_simulator), ), # The `name` keyword overrides the name for all created nodes, so duplicated nodes appear. From d057ca7ba37bd2a159912267a5c12dfbf20e7ed6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Thu, 7 Dec 2023 10:03:18 +0100 Subject: [PATCH 03/12] Possibility of changing SimulationClock::realtime_factor during the simulation with ROS 2 topic --- .../include/traffic_simulator/api/api.hpp | 7 +++++ .../simulation_clock/simulation_clock.hpp | 30 +++++++++++++++---- .../src/simulation_clock/simulation_clock.cpp | 12 ++++++-- 3 files changed, 42 insertions(+), 7 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 03f168c0fbe..a7fdc6ba808 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -80,6 +81,10 @@ class API { setVerbose(configuration.verbose); + real_time_factor_subscriber = rclcpp::create_subscription( + node, "/real_time_factor", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), + [this](const std_msgs::msg::Float64 & message) { clock_.setRealTimeFactor(message.data); }); + if (not configuration.standalone_mode) { simulation_api_schema::InitializeRequest request; request.set_initialize_time(clock_.getCurrentSimulationTime()); @@ -368,6 +373,8 @@ class API const rclcpp::Publisher::SharedPtr debug_marker_pub_; + rclcpp::Subscription::SharedPtr real_time_factor_subscriber; + SimulationClock clock_; zeromq::MultiClient zeromq_client_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp index 62a3a105c01..83735b3b87f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp @@ -29,18 +29,34 @@ class SimulationClock : rclcpp::Clock auto getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock; - auto getCurrentScenarioTime() const { return (frame_ - frame_offset_) / frame_rate * realtime_factor; } + auto getCurrentScenarioTime() const + { + auto nanoseconds = nanoseconds_count - nanoseconds_offset; + auto seconds = seconds_count - seconds_offset; - auto getCurrentSimulationTime() const { return frame_ / frame_rate * realtime_factor; } + if (nanoseconds < 0) { + seconds -= 1; + nanoseconds += 1000000000; + } + + return nanoseconds / 1000000000.0 + seconds; + } + + auto getCurrentSimulationTime() const { return nanoseconds_count / 1000000000.0 + seconds_count; } auto getStepTime() const { return realtime_factor / frame_rate; } auto start() -> void; - auto started() const { return not std::isnan(frame_offset_); } + auto started() const + { + return not std::isnan(nanoseconds_offset) || not std::isnan(seconds_offset); + } auto update() -> void; + auto setRealTimeFactor(double realtime_factor_) { realtime_factor = realtime_factor_; }; + const bool use_raw_clock; double realtime_factor; @@ -50,9 +66,13 @@ class SimulationClock : rclcpp::Clock const rclcpp::Time time_on_initialize; private: - double frame_ = 0; + double seconds_count = 0; + + double nanoseconds_count = 0; + + double seconds_offset = std::numeric_limits::quiet_NaN(); - double frame_offset_ = std::numeric_limits::quiet_NaN(); + double nanoseconds_offset = std::numeric_limits::quiet_NaN(); }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp index 9cce02cb367..3abbfe09591 100644 --- a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp +++ b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp @@ -26,7 +26,14 @@ SimulationClock::SimulationClock(double realtime_factor, double frame_rate) { } -auto SimulationClock::update() -> void { ++frame_; } +auto SimulationClock::update() -> void +{ + nanoseconds_count += realtime_factor * 1000000000.0 / frame_rate; + + auto full_seconds = std::floor(nanoseconds_count / 1000000000.0); + nanoseconds_count -= full_seconds * 1000000000.0; + seconds_count += full_seconds; +} auto SimulationClock::getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock { @@ -50,7 +57,8 @@ auto SimulationClock::start() -> void THROW_SIMULATION_ERROR( "npc logic is already started. Please check simulation clock instance was destroyed."); } else { - frame_offset_ = frame_; + seconds_offset = seconds_count; + nanoseconds_offset = nanoseconds_count; } } } // namespace traffic_simulator From cf57ed5e685e0420320de94abb27d4b5c5e44e1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Tue, 12 Dec 2023 13:49:07 +0100 Subject: [PATCH 04/12] UpdateStepTime request for updating simple sensor simulation step_time MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../simple_sensor_simulator.hpp | 3 +++ .../src/simple_sensor_simulator.cpp | 12 +++++++++++- .../simulation_interface/zmq_multi_client.hpp | 3 +++ .../simulation_interface/zmq_multi_server.hpp | 3 ++- .../proto/simulation_api_schema.proto | 16 ++++++++++++++++ .../src/zmq_multi_client.cpp | 12 ++++++++++++ .../src/zmq_multi_server.cpp | 4 ++++ .../include/traffic_simulator/api/api.hpp | 8 +++++++- 8 files changed, 58 insertions(+), 3 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp index 5edd84e8a29..8547427f2ff 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp +++ b/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; diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index aafab6ef288..45918c55e06 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/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(xs)...); }, [this](auto &&... xs) { return attachPseudoTrafficLightDetector(std::forward(xs)...); - }) + }, + [this](auto &&... xs) { return updateStepTime(std::forward(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 diff --git a/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp b/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp index b33463f09b6..0d14b562aab 100644 --- a/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp +++ b/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; diff --git a/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp b/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp index 0eb17135ffd..97e4f24f8c5 100644 --- a/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp +++ b/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp @@ -73,6 +73,7 @@ class MultiServer DEFINE_FUNCTION_TYPE(UpdateTrafficLights); DEFINE_FUNCTION_TYPE(FollowPolylineTrajectory); DEFINE_FUNCTION_TYPE(AttachPseudoTrafficLightDetector); + DEFINE_FUNCTION_TYPE(UpdateStepTime); #undef DEFINE_FUNCTION_TYPE @@ -80,7 +81,7 @@ class MultiServer Initialize, UpdateFrame, SpawnVehicleEntity, SpawnPedestrianEntity, SpawnMiscObjectEntity, DespawnEntity, UpdateEntityStatus, AttachLidarSensor, AttachDetectionSensor, AttachOccupancyGridSensor, UpdateTrafficLights, FollowPolylineTrajectory, - AttachPseudoTrafficLightDetector> + AttachPseudoTrafficLightDetector, UpdateStepTime> functions_; }; } // namespace zeromq diff --git a/simulation/simulation_interface/proto/simulation_api_schema.proto b/simulation/simulation_interface/proto/simulation_api_schema.proto index 5fe4135087f..7f1c3f80d87 100644 --- a/simulation/simulation_interface/proto/simulation_api_schema.proto +++ b/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; } } diff --git a/simulation/simulation_interface/src/zmq_multi_client.cpp b/simulation/simulation_interface/src/zmq_multi_client.cpp index ef339d1fa26..32483feaa86 100644 --- a/simulation/simulation_interface/src/zmq_multi_client.cpp +++ b/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 { diff --git a/simulation/simulation_interface/src/zmq_multi_server.cpp b/simulation/simulation_interface/src/zmq_multi_server.cpp index 4b59f731e1d..eb4c8211857 100644 --- a/simulation/simulation_interface/src/zmq_multi_server.cpp +++ b/simulation/simulation_interface/src/zmq_multi_server.cpp @@ -82,6 +82,10 @@ void MultiServer::poll() std::get(functions_)( proto.attach_pseudo_traffic_light_detector()); break; + case simulation_api_schema::SimulationRequest::RequestCase::kUpdateStepTime: + *sim_response.mutable_update_step_time() = + std::get(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"); } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index a7fdc6ba808..7398bc3726b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -83,7 +83,13 @@ class API real_time_factor_subscriber = rclcpp::create_subscription( node, "/real_time_factor", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), - [this](const std_msgs::msg::Float64 & message) { clock_.setRealTimeFactor(message.data); }); + [this](const std_msgs::msg::Float64 & message) { + clock_.setRealTimeFactor(message.data); + + simulation_api_schema::UpdateStepTimeRequest request; + request.set_simulation_step_time(clock_.getStepTime()); + zeromq_client_.call(request); + }); if (not configuration.standalone_mode) { simulation_api_schema::InitializeRequest request; From 5274218bb30809eb396cb81efb553b5ded55a3a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 13 Dec 2023 13:04:26 +0100 Subject: [PATCH 05/12] RViz plugin controlling real time factor value MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../CMakeLists.txt | 27 ++++++++ .../package.xml | 28 ++++++++ .../plugins/plugin_description.xml | 9 +++ .../src/real_time_factor_slider.cpp | 64 +++++++++++++++++++ .../src/real_time_factor_slider.hpp | 54 ++++++++++++++++ .../include/traffic_simulator/api/api.hpp | 4 +- 6 files changed, 185 insertions(+), 1 deletion(-) create mode 100644 rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt create mode 100644 rviz_plugins/real_time_factor_control_rviz_plugin/package.xml create mode 100644 rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml create mode 100644 rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp create mode 100644 rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt b/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt new file mode 100644 index 00000000000..7da43b69669 --- /dev/null +++ b/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 +) diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml new file mode 100644 index 00000000000..c249c810d83 --- /dev/null +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -0,0 +1,28 @@ + + + + real_time_factor_control_rviz_plugin + 0.1.0 + Slider controlling real time factor value. + Paweł Lech + Apache License 2.0 + + Paweł Lech + + ament_cmake_auto + autoware_cmake + + libqt5-core + libqt5-widgets + qtbase5-dev + rviz_common + std_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 00000000000..03d6f046b90 --- /dev/null +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + Slider controlling real time factor value. + + + diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp new file mode 100644 index 00000000000..c0aa37178cd --- /dev/null +++ b/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("/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) diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp new file mode 100644 index 00000000000..539a919fbdb --- /dev/null +++ b/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::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_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 7398bc3726b..816b184e4f5 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -84,8 +84,10 @@ class API real_time_factor_subscriber = rclcpp::create_subscription( 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); From 3b4b5472ef15bf07e5f35a65231f74df1087178e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Mon, 18 Dec 2023 18:57:14 +0100 Subject: [PATCH 06/12] Corrected time storage in Simulation Clock. Other minor changes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../src/real_time_factor_slider.cpp | 8 ++-- .../src/real_time_factor_slider.hpp | 12 +++--- .../include/traffic_simulator/api/api.hpp | 2 +- .../simulation_clock/simulation_clock.hpp | 41 ++++++------------- .../src/simulation_clock/simulation_clock.cpp | 24 ++++------- 5 files changed, 31 insertions(+), 56 deletions(-) diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp index c0aa37178cd..b3fc73a313a 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp @@ -14,11 +14,11 @@ // limitations under the License. // -#include "./real_time_factor_slider.hpp" +#include "real_time_factor_slider.hpp" -#include "QHBoxLayout" -#include "pluginlib/class_list_macros.hpp" -#include "rviz_common/display_context.hpp" +#include +#include +#include namespace real_time_factor_control_rviz_plugin { diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp index 539a919fbdb..77657a8f1d0 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp @@ -17,17 +17,17 @@ #ifndef REAL_TIME_FACTOR_SLIDER_PANEL_HPP_ #define REAL_TIME_FACTOR_SLIDER_PANEL_HPP_ -#include "QLabel" -#include "QSlider" +#include +#include #ifndef Q_MOC_RUN -#include "rclcpp/rclcpp.hpp" -#include "rviz_common/panel.hpp" +#include +#include #endif -#include "std_msgs/msg/float64.hpp" +#include namespace real_time_factor_control_rviz_plugin { @@ -37,7 +37,7 @@ class RealTimeFactorSliderPanel : public rviz_common::Panel public: explicit RealTimeFactorSliderPanel(QWidget * parent = nullptr); - void onInitialize() override; + auto onInitialize() -> void override; public Q_SLOTS: void onChangedRealTimeFactorValue(int real_time_factor_value); diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 816b184e4f5..8336a18d845 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -97,7 +97,7 @@ class API simulation_api_schema::InitializeRequest request; request.set_initialize_time(clock_.getCurrentSimulationTime()); request.set_lanelet2_map_path(configuration.lanelet2_map_path().string()); - request.set_realtime_factor(clock_.realtime_factor); + request.set_realtime_factor(clock_.realtime_factor_); request.set_step_time(clock_.getStepTime()); simulation_interface::toProto( clock_.getCurrentRosTime(), *request.mutable_initialize_ros_time()); diff --git a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp index 83735b3b87f..b614555a910 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp @@ -29,50 +29,33 @@ class SimulationClock : rclcpp::Clock auto getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock; - auto getCurrentScenarioTime() const - { - auto nanoseconds = nanoseconds_count - nanoseconds_offset; - auto seconds = seconds_count - seconds_offset; + auto getCurrentScenarioTime() const { return time_ - time_offset_; } - if (nanoseconds < 0) { - seconds -= 1; - nanoseconds += 1000000000; - } + auto getCurrentSimulationTime() const { return time_; } - return nanoseconds / 1000000000.0 + seconds; - } - - auto getCurrentSimulationTime() const { return nanoseconds_count / 1000000000.0 + seconds_count; } - - auto getStepTime() const { return realtime_factor / frame_rate; } + auto getStepTime() const { return realtime_factor_ / frame_rate_; } auto start() -> void; - auto started() const - { - return not std::isnan(nanoseconds_offset) || not std::isnan(seconds_offset); - } + auto started() const { return not std::isnan(time_offset_); } auto update() -> void; - auto setRealTimeFactor(double realtime_factor_) { realtime_factor = realtime_factor_; }; + auto setRealTimeFactor(double realtime_factor) { realtime_factor_ = realtime_factor; }; - const bool use_raw_clock; + const bool use_raw_clock_; - double realtime_factor; + double realtime_factor_; - double frame_rate; + double frame_rate_; - const rclcpp::Time time_on_initialize; + const rclcpp::Time time_on_initialize_; private: - double seconds_count = 0; - - double nanoseconds_count = 0; - - double seconds_offset = std::numeric_limits::quiet_NaN(); + // time in seconds + double time_ = 0.0; - double nanoseconds_offset = std::numeric_limits::quiet_NaN(); + double time_offset_ = std::numeric_limits::quiet_NaN(); }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp index 3abbfe09591..7cf8a383cc4 100644 --- a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp +++ b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp @@ -19,21 +19,14 @@ namespace traffic_simulator { SimulationClock::SimulationClock(double realtime_factor, double frame_rate) : rclcpp::Clock(RCL_ROS_TIME), - use_raw_clock(false), - realtime_factor(realtime_factor), - frame_rate(frame_rate), - time_on_initialize(0, 0) + use_raw_clock_(false), + realtime_factor_(realtime_factor), + frame_rate_(frame_rate), + time_on_initialize_(0, 0) { } -auto SimulationClock::update() -> void -{ - nanoseconds_count += realtime_factor * 1000000000.0 / frame_rate; - - auto full_seconds = std::floor(nanoseconds_count / 1000000000.0); - nanoseconds_count -= full_seconds * 1000000000.0; - seconds_count += full_seconds; -} +auto SimulationClock::update() -> void { time_ += realtime_factor_ / frame_rate_; } auto SimulationClock::getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock { @@ -44,10 +37,10 @@ auto SimulationClock::getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock auto SimulationClock::getCurrentRosTime() -> rclcpp::Time { - if (use_raw_clock) { + if (use_raw_clock_) { return now(); } else { - return time_on_initialize + rclcpp::Duration::from_seconds(getCurrentSimulationTime()); + return time_on_initialize_ + rclcpp::Duration::from_seconds(getCurrentSimulationTime()); } } @@ -57,8 +50,7 @@ auto SimulationClock::start() -> void THROW_SIMULATION_ERROR( "npc logic is already started. Please check simulation clock instance was destroyed."); } else { - seconds_offset = seconds_count; - nanoseconds_offset = nanoseconds_count; + time_offset_ = time_; } } } // namespace traffic_simulator From 0970a4c2218135844b1373a6e65c063c64bf4913 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Tue, 19 Dec 2023 12:31:03 +0100 Subject: [PATCH 07/12] Setting concealer use_sim_time manually instead of using global arguments. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- external/concealer/src/autoware.cpp | 2 +- .../src/real_time_factor_slider.cpp | 2 +- .../vehicle_simulation/ego_entity_simulation.hpp | 2 +- .../simple_sensor_simulator/src/simple_sensor_simulator.cpp | 3 ++- .../src/vehicle_simulation/ego_entity_simulation.cpp | 4 +++- .../launch/scenario_test_runner.launch.py | 5 ++--- 6 files changed, 10 insertions(+), 8 deletions(-) diff --git a/external/concealer/src/autoware.cpp b/external/concealer/src/autoware.cpp index 2adc692db72..a209e4c8f84 100644 --- a/external/concealer/src/autoware.cpp +++ b/external/concealer/src/autoware.cpp @@ -17,7 +17,7 @@ namespace concealer { Autoware::Autoware() -: rclcpp::Node("concealer", "simulation", rclcpp::NodeOptions().use_global_arguments(true)), +: rclcpp::Node("concealer", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), current_acceleration(geometry_msgs::msg::Accel()), current_twist(geometry_msgs::msg::Twist()), current_pose(geometry_msgs::msg::Pose()) diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp index b3fc73a313a..0bcfe99474b 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp @@ -48,7 +48,7 @@ void RealTimeFactorSliderPanel::onChangedRealTimeFactorValue(int real_time_facto value_label_->setText(QString("x ") + QString::number(msg.data, 'f', 2)); } -void RealTimeFactorSliderPanel::onInitialize() +auto RealTimeFactorSliderPanel::onInitialize() -> void { rclcpp::Node::SharedPtr raw_node = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index e6cd5a4579f..f23176cbfe8 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -78,7 +78,7 @@ class EgoEntitySimulation explicit EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters &, double, - const std::shared_ptr &); + const std::shared_ptr &, const rclcpp::Parameter & use_sim_time); auto update(double time, double step_time, bool npc_logic_started) -> void; diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index 45918c55e06..798c9871a25 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -208,7 +208,8 @@ auto ScenarioSimulator::spawnVehicleEntity( traffic_simulator_msgs::msg::VehicleParameters parameters; simulation_interface::toMsg(req.parameters(), parameters); ego_entity_simulation_ = std::make_shared( - 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); diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index 9b39a8f108a..2d4fafb8544 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -33,12 +33,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) + const std::shared_ptr & hdmap_utils, + const rclcpp::Parameter & use_sim_time) : autoware(std::make_unique()), 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 diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 66255687dfc..5f82d8b03d5 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -171,8 +171,7 @@ def description(): namespace="simulation", output="screen", on_exit=ShutdownOnce(), - parameters=[{"port": port}]+make_vehicle_parameters()+[{"use_sim_time": True}], - # parameters=[{"port": port}]+make_vehicle_parameters(), + parameters=[{"port": port}, {"use_sim_time": True}]+make_vehicle_parameters(), condition=IfCondition(launch_simple_sensor_simulator), ), # The `name` keyword overrides the name for all created nodes, so duplicated nodes appear. @@ -185,7 +184,7 @@ def description(): executable="openscenario_interpreter_node", namespace="simulation", output="screen", - parameters=make_parameters(), + parameters=[{"use_sim_time": True}]+make_parameters(), on_exit=ShutdownOnce(), ), Node( From a43e553881fdcd65dace994da7e88704bd377096 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Tue, 19 Dec 2023 14:59:45 +0100 Subject: [PATCH 08/12] Removed autoware_cmake dependency from real time factor control rviz plugin MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../real_time_factor_control_rviz_plugin/CMakeLists.txt | 9 +++------ .../real_time_factor_control_rviz_plugin/package.xml | 2 +- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt b/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt index 7da43b69669..60600991ba3 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt @@ -1,8 +1,8 @@ cmake_minimum_required(VERSION 3.14) project(real_time_factor_control_rviz_plugin) -find_package(autoware_cmake REQUIRED) -autoware_package() +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) @@ -21,7 +21,4 @@ target_link_libraries(${PROJECT_NAME} # 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 -) +ament_auto_package() diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index c249c810d83..d9fd6632fec 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -9,8 +9,8 @@ Paweł Lech + ament_cmake ament_cmake_auto - autoware_cmake libqt5-core libqt5-widgets From c5c5d8ed8902a694fbeb83b29e2c4a29a2bc2825 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Thu, 21 Dec 2023 18:12:16 +0100 Subject: [PATCH 09/12] use_sim_time for openscenario_interpreter is parameterized and False by default MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 2 +- .../traffic_simulator/simulation_clock/simulation_clock.hpp | 2 +- .../src/simulation_clock/simulation_clock.cpp | 6 +++--- .../launch/scenario_test_runner.launch.py | 5 ++++- 4 files changed, 9 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 8336a18d845..95200bb3fbc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -75,7 +75,7 @@ class API rclcpp::PublisherOptionsWithAllocator())), debug_marker_pub_(rclcpp::create_publisher( node, "debug_marker", rclcpp::QoS(100), rclcpp::PublisherOptionsWithAllocator())), - clock_(std::forward(xs)...), + clock_(node->get_parameter("use_sim_time").as_bool(), std::forward(xs)...), zeromq_client_( simulation_interface::protocol, configuration.simulator_host, getZMQSocketPort(*node)) { diff --git a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp index b614555a910..614fb045d98 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp @@ -23,7 +23,7 @@ namespace traffic_simulator class SimulationClock : rclcpp::Clock { public: - explicit SimulationClock(double realtime_factor, double frame_rate); + explicit SimulationClock(bool use_sim_time, double realtime_factor, double frame_rate); auto getCurrentRosTime() -> rclcpp::Time; diff --git a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp index 7cf8a383cc4..6ae9afc4f61 100644 --- a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp +++ b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp @@ -17,12 +17,12 @@ namespace traffic_simulator { -SimulationClock::SimulationClock(double realtime_factor, double frame_rate) +SimulationClock::SimulationClock(bool use_sim_time, double realtime_factor, double frame_rate) : rclcpp::Clock(RCL_ROS_TIME), - use_raw_clock_(false), + use_raw_clock_(!use_sim_time), realtime_factor_(realtime_factor), frame_rate_(frame_rate), - time_on_initialize_(0, 0) + time_on_initialize_(use_sim_time ? 0 : now().nanoseconds()) { } diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 5f82d8b03d5..c32d52fe256 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -79,6 +79,7 @@ def launch_setup(context, *args, **kwargs): sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) vehicle_model = LaunchConfiguration("vehicle_model", default="") workflow = LaunchConfiguration("workflow", default=Path("/dev/null")) + use_simulation_time = LaunchConfiguration("use_simulation_time", default=False) # fmt: on print(f"architecture_type := {architecture_type.perform(context)}") @@ -99,6 +100,7 @@ def launch_setup(context, *args, **kwargs): print(f"sigterm_timeout := {sigterm_timeout.perform(context)}") print(f"vehicle_model := {vehicle_model.perform(context)}") print(f"workflow := {workflow.perform(context)}") + print(f"use_simulation_time := {use_simulation_time.perform(context)}") def make_parameters(): parameters = [ @@ -146,6 +148,7 @@ def description(): DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), DeclareLaunchArgument("workflow", default_value=workflow ), + DeclareLaunchArgument("use_simulation_time", default_value=use_simulation_time ), # fmt: on Node( package="scenario_test_runner", @@ -184,7 +187,7 @@ def description(): executable="openscenario_interpreter_node", namespace="simulation", output="screen", - parameters=[{"use_sim_time": True}]+make_parameters(), + parameters=[{"use_sim_time": use_simulation_time}]+make_parameters(), on_exit=ShutdownOnce(), ), Node( From dc0bac4771a690b587338d43723d1e1ea6f0b849 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 17 Jan 2024 11:52:20 +0100 Subject: [PATCH 10/12] Changes after review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../CMakeLists.txt | 17 +++++----- .../plugins/plugin_description.xml | 8 ++--- .../src/real_time_factor_slider.cpp | 34 +++++++++---------- .../src/real_time_factor_slider.hpp | 20 ++++++----- .../include/traffic_simulator/api/api.hpp | 30 ++++++++-------- .../simulation_clock/simulation_clock.hpp | 26 +++++++------- .../src/simulation_clock/simulation_clock.cpp | 18 ++++++---- .../launch/scenario_test_runner.launch.py | 8 ++--- 8 files changed, 84 insertions(+), 77 deletions(-) diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt b/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt index 60600991ba3..978867f719b 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt @@ -1,22 +1,23 @@ 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() -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 -) +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} -) +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) diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml index 03d6f046b90..59119575da2 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml @@ -1,9 +1,7 @@ - - Slider controlling real time factor value. - + type="real_time_factor_control_rviz_plugin::RealTimeFactorSliderPanel" + base_class_type="rviz_common::Panel"> + Slider controlling real time factor value. diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp index 0bcfe99474b..5f3a0199389 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp @@ -14,51 +14,51 @@ // limitations under the License. // -#include "real_time_factor_slider.hpp" - #include #include +#include #include namespace real_time_factor_control_rviz_plugin { -RealTimeFactorSliderPanel::RealTimeFactorSliderPanel(QWidget * parent) : rviz_common::Panel(parent) +RealTimeFactorSliderPanel::RealTimeFactorSliderPanel(QWidget * parent) +: rviz_common::Panel(parent), + value_label_(new QLabel("x 1.00")), + slider_(new QSlider(Qt::Horizontal)) { - 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(); + auto layout = new QHBoxLayout(this); layout->addWidget(value_label_); layout->addWidget(slider_); setLayout(layout); } -void RealTimeFactorSliderPanel::onChangedRealTimeFactorValue(int real_time_factor_value) +auto RealTimeFactorSliderPanel::onChangedRealTimeFactorValue(int percentage) -> void { - 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)); + 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 { - rclcpp::Node::SharedPtr raw_node = - this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - update_real_time_factor_publisher = - raw_node->create_publisher("/real_time_factor", 1); + real_time_factor_publisher = getDisplayContext() + ->getRosNodeAbstraction() + .lock() + ->get_raw_node() + ->create_publisher("/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) + real_time_factor_control_rviz_plugin::RealTimeFactorSliderPanel, rviz_common::Panel) \ No newline at end of file diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp index 77657a8f1d0..671b349604e 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp @@ -19,16 +19,13 @@ #include #include +#include #ifndef Q_MOC_RUN - #include #include - #endif -#include - namespace real_time_factor_control_rviz_plugin { class RealTimeFactorSliderPanel : public rviz_common::Panel @@ -37,18 +34,23 @@ class RealTimeFactorSliderPanel : public rviz_common::Panel 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::SharedPtr update_real_time_factor_publisher; + rclcpp::Publisher::SharedPtr real_time_factor_publisher; - QLabel * value_label_; - QSlider * slider_; -}; + QLabel * const value_label_; -} // end namespace real_time_factor_control_rviz_plugin + QSlider * const slider_; +}; +} // namespace real_time_factor_control_rviz_plugin #endif // REAL_TIME_FACTOR_SLIDER_PANEL_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 1c847f4788b..12e9bc0a19e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -75,29 +75,31 @@ class API rclcpp::PublisherOptionsWithAllocator())), debug_marker_pub_(rclcpp::create_publisher( node, "debug_marker", rclcpp::QoS(100), rclcpp::PublisherOptionsWithAllocator())), + real_time_factor_subscriber(rclcpp::create_subscription( + node, "/real_time_factor", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), + [this](const std_msgs::msg::Float64 & message) { + /* + 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(xs)...), zeromq_client_( simulation_interface::protocol, configuration.simulator_host, getZMQSocketPort(*node)) { setVerbose(configuration.verbose); - real_time_factor_subscriber = rclcpp::create_subscription( - 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); - }); - if (not configuration.standalone_mode) { simulation_api_schema::InitializeRequest request; request.set_initialize_time(clock_.getCurrentSimulationTime()); request.set_lanelet2_map_path(configuration.lanelet2_map_path().string()); - request.set_realtime_factor(clock_.realtime_factor_); + request.set_realtime_factor(clock_.realtime_factor); request.set_step_time(clock_.getStepTime()); simulation_interface::toProto( clock_.getCurrentRosTime(), *request.mutable_initialize_ros_time()); @@ -383,7 +385,7 @@ class API const rclcpp::Publisher::SharedPtr debug_marker_pub_; - rclcpp::Subscription::SharedPtr real_time_factor_subscriber; + const rclcpp::Subscription::SharedPtr real_time_factor_subscriber; SimulationClock clock_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp index 614fb045d98..94f6642638c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp @@ -29,33 +29,33 @@ class SimulationClock : rclcpp::Clock auto getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock; - auto getCurrentScenarioTime() const { return time_ - time_offset_; } + auto getCurrentScenarioTime() const + { + return seconds_since_the_simulator_started_ - seconds_at_the_start_of_the_scenario_; + } - auto getCurrentSimulationTime() const { return time_; } + auto getCurrentSimulationTime() const { return seconds_since_the_simulator_started_; } - auto getStepTime() const { return realtime_factor_ / frame_rate_; } + auto getStepTime() const { return realtime_factor / frame_rate_; } auto start() -> void; - auto started() const { return not std::isnan(time_offset_); } + auto started() const { return not std::isnan(seconds_at_the_start_of_the_scenario_); } auto update() -> void; - auto setRealTimeFactor(double realtime_factor) { realtime_factor_ = realtime_factor; }; + const bool use_sim_time; - const bool use_raw_clock_; - - double realtime_factor_; + double realtime_factor; +private: double frame_rate_; - const rclcpp::Time time_on_initialize_; + const rclcpp::Time time_at_the_start_of_the_simulator_; -private: - // time in seconds - double time_ = 0.0; + double seconds_since_the_simulator_started_ = 0.0; - double time_offset_ = std::numeric_limits::quiet_NaN(); + double seconds_at_the_start_of_the_scenario_ = std::numeric_limits::quiet_NaN(); }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp index 6ae9afc4f61..30f1c5081bf 100644 --- a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp +++ b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp @@ -19,14 +19,17 @@ namespace traffic_simulator { SimulationClock::SimulationClock(bool use_sim_time, double realtime_factor, double frame_rate) : rclcpp::Clock(RCL_ROS_TIME), - use_raw_clock_(!use_sim_time), - realtime_factor_(realtime_factor), + use_sim_time(use_sim_time), + realtime_factor(realtime_factor), frame_rate_(frame_rate), - time_on_initialize_(use_sim_time ? 0 : now().nanoseconds()) + time_at_the_start_of_the_simulator_(use_sim_time ? 0 : now().nanoseconds()) { } -auto SimulationClock::update() -> void { time_ += realtime_factor_ / frame_rate_; } +auto SimulationClock::update() -> void +{ + seconds_since_the_simulator_started_ += realtime_factor / frame_rate_; +} auto SimulationClock::getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock { @@ -37,10 +40,11 @@ auto SimulationClock::getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock auto SimulationClock::getCurrentRosTime() -> rclcpp::Time { - if (use_raw_clock_) { + if (not use_sim_time) { return now(); } else { - return time_on_initialize_ + rclcpp::Duration::from_seconds(getCurrentSimulationTime()); + return time_at_the_start_of_the_simulator_ + + rclcpp::Duration::from_seconds(getCurrentSimulationTime()); } } @@ -50,7 +54,7 @@ auto SimulationClock::start() -> void THROW_SIMULATION_ERROR( "npc logic is already started. Please check simulation clock instance was destroyed."); } else { - time_offset_ = time_; + seconds_at_the_start_of_the_scenario_ = seconds_since_the_simulator_started_; } } } // namespace traffic_simulator diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index c32d52fe256..2c2e6973d08 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -77,9 +77,9 @@ def launch_setup(context, *args, **kwargs): scenario = LaunchConfiguration("scenario", default=Path("/dev/null")) sensor_model = LaunchConfiguration("sensor_model", default="") sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) + use_sim_time = LaunchConfiguration("use_sim_time", default=False) vehicle_model = LaunchConfiguration("vehicle_model", default="") workflow = LaunchConfiguration("workflow", default=Path("/dev/null")) - use_simulation_time = LaunchConfiguration("use_simulation_time", default=False) # fmt: on print(f"architecture_type := {architecture_type.perform(context)}") @@ -98,9 +98,9 @@ def launch_setup(context, *args, **kwargs): print(f"scenario := {scenario.perform(context)}") print(f"sensor_model := {sensor_model.perform(context)}") print(f"sigterm_timeout := {sigterm_timeout.perform(context)}") + print(f"use_sim_time := {use_sim_time.perform(context)}") print(f"vehicle_model := {vehicle_model.perform(context)}") print(f"workflow := {workflow.perform(context)}") - print(f"use_simulation_time := {use_simulation_time.perform(context)}") def make_parameters(): parameters = [ @@ -146,9 +146,9 @@ def description(): DeclareLaunchArgument("scenario", default_value=scenario ), DeclareLaunchArgument("sensor_model", default_value=sensor_model ), DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), + DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ), DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), DeclareLaunchArgument("workflow", default_value=workflow ), - DeclareLaunchArgument("use_simulation_time", default_value=use_simulation_time ), # fmt: on Node( package="scenario_test_runner", @@ -187,7 +187,7 @@ def description(): executable="openscenario_interpreter_node", namespace="simulation", output="screen", - parameters=[{"use_sim_time": use_simulation_time}]+make_parameters(), + parameters=[{"use_sim_time": use_sim_time}]+make_parameters(), on_exit=ShutdownOnce(), ), Node( From e7683414bdd40529701334517ecdf5f83bdb6cde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Wed, 17 Jan 2024 12:02:02 +0100 Subject: [PATCH 11/12] New line at the EOF MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../src/real_time_factor_slider.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp index 5f3a0199389..1d4aa9bd4cd 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp @@ -61,4 +61,4 @@ auto RealTimeFactorSliderPanel::onInitialize() -> void } // namespace real_time_factor_control_rviz_plugin PLUGINLIB_EXPORT_CLASS( - real_time_factor_control_rviz_plugin::RealTimeFactorSliderPanel, rviz_common::Panel) \ No newline at end of file + real_time_factor_control_rviz_plugin::RealTimeFactorSliderPanel, rviz_common::Panel) From 097fd66ea9a37d0488efdf6853b5c030c41050ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Lech?= Date: Tue, 6 Feb 2024 08:41:22 +0100 Subject: [PATCH 12/12] Doxygen note MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Lech --- .../include/traffic_simulator/api/api.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 12e9bc0a19e..19f69e6e105 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -78,10 +78,10 @@ class API real_time_factor_subscriber(rclcpp::create_subscription( node, "/real_time_factor", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), [this](const std_msgs::msg::Float64 & message) { - /* - 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. - */ + /** + * @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;