From b172360354e466b172d6563875f1f4a68a5eca58 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 24 Aug 2023 17:52:09 +0900 Subject: [PATCH 01/27] Add some comments Signed-off-by: yamacir-kit --- .../concealer/src/field_operator_application.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index acccd31fee0..0d5b654edd0 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -106,16 +106,30 @@ auto FieldOperatorApplication::shutdownAutoware() -> void while (sigtimedwait(&mask, NULL, &timeout) < 0) { switch (errno) { - case EINTR: // Interrupted by a signal other than SIGCHLD. + case EINTR: + /* + The wait was interrupted by an unblocked, caught signal. It + shall be documented in system documentation whether this error + causes these functions to fail. + */ break; case EAGAIN: + /* + No signal specified by set was generated within the specified + timeout period. + */ AUTOWARE_ERROR_STREAM( "Shutting down Autoware: (2/3) Autoware launch process does not respond. Kill it."); kill(process_id, SIGKILL); break; default: + case EINVAL: + /* + The timeout argument specified a tv_nsec value less than zero or + greater than or equal to 1000 million. + */ AUTOWARE_SYSTEM_ERROR("sigtimedwait"); std::exit(EXIT_FAILURE); } From fd7765d589caa86a90ed83812362271c6ab8209a Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 24 Aug 2023 18:56:31 +0900 Subject: [PATCH 02/27] Update `shutdownAutoware` to respect the parameter `sigterm_timeout` Signed-off-by: yamacir-kit --- .../src/field_operator_application.cpp | 43 ++++++++++++------- .../launch/scenario_test_runner.launch.py | 5 ++- 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 0d5b654edd0..7d24bd9132a 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -16,6 +16,7 @@ #include #include #include +#include namespace concealer { @@ -85,26 +86,36 @@ auto FieldOperatorApplication::shutdownAutoware() -> void AUTOWARE_INFO_STREAM("Shutting down Autoware: (2/3) Terminating Autoware."); { - sigset_t mask{}; - { - sigset_t orig_mask{}; - - sigemptyset(&mask); - sigaddset(&mask, SIGCHLD); - - if (sigprocmask(SIG_BLOCK, &mask, &orig_mask) < 0) { - AUTOWARE_SYSTEM_ERROR("sigprocmask"); + const auto sigset = [this]() { + if (auto signal_set = sigset_t(); + sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) { + RCLCPP_ERROR_STREAM( + get_logger(), std::system_error(errno, std::system_category()).what()); std::exit(EXIT_FAILURE); + } else if (auto error = pthread_sigmask(SIG_BLOCK, &signal_set, nullptr)) { + RCLCPP_ERROR_STREAM( + get_logger(), std::system_error(error, std::system_category()).what()); + std::exit(EXIT_FAILURE); + } else { + return signal_set; } - } - - timespec timeout{}; - { - timeout.tv_sec = 5; + }(); + + const auto timeout = [this]() { + auto sigterm_timeout = [this](auto value) { + auto node = rclcpp::Node("get_parameter_sigterm_timeout", "simulation"); + node.declare_parameter("sigterm_timeout", value); + node.get_parameter("sigterm_timeout", value); + return value; + }; + auto timeout = timespec(); + timeout.tv_sec = sigterm_timeout(0); timeout.tv_nsec = 0; - } + return timeout; + }(); - while (sigtimedwait(&mask, NULL, &timeout) < 0) { + while ((0 < timeout.tv_sec ? sigtimedwait(&sigset, nullptr, &timeout) + : sigwaitinfo(&sigset, nullptr)) < 0) { switch (errno) { case EINTR: /* 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 058025b9f0a..d877249c412 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 @@ -109,11 +109,12 @@ def make_parameters(): {"record": record}, {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, + {"sigterm_timeout": sigterm_timeout}, {"vehicle_model": vehicle_model}, ] parameters += make_vehicle_parameters() return parameters - + def make_vehicle_parameters(): parameters = [] @@ -170,7 +171,7 @@ def description(): name="simple_sensor_simulator", output="screen", on_exit=ShutdownOnce(), - parameters=[{"port": port}]+make_vehicle_parameters(), + parameters=make_parameters(), condition=IfCondition(launch_simple_sensor_simulator), ), LifecycleNode( From fbdf15a3786b6fd78725abeb276c126cae1979cb Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 24 Aug 2023 19:19:23 +0900 Subject: [PATCH 03/27] Cleanup error messages Signed-off-by: yamacir-kit --- .../src/field_operator_application.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 7d24bd9132a..83bf6d7543d 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -109,13 +109,12 @@ auto FieldOperatorApplication::shutdownAutoware() -> void return value; }; auto timeout = timespec(); - timeout.tv_sec = sigterm_timeout(0); + timeout.tv_sec = sigterm_timeout(5); timeout.tv_nsec = 0; return timeout; }(); - while ((0 < timeout.tv_sec ? sigtimedwait(&sigset, nullptr, &timeout) - : sigwaitinfo(&sigset, nullptr)) < 0) { + if (sigtimedwait(&sigset, nullptr, &timeout) < 0) { switch (errno) { case EINTR: /* @@ -123,6 +122,10 @@ auto FieldOperatorApplication::shutdownAutoware() -> void shall be documented in system documentation whether this error causes these functions to fail. */ + RCLCPP_ERROR_STREAM( + get_logger(), + "The wait for Autoware launch process termination was interrupted by an unblocked, " + "caught signal."); break; case EAGAIN: @@ -130,8 +133,7 @@ auto FieldOperatorApplication::shutdownAutoware() -> void No signal specified by set was generated within the specified timeout period. */ - AUTOWARE_ERROR_STREAM( - "Shutting down Autoware: (2/3) Autoware launch process does not respond. Kill it."); + RCLCPP_ERROR_STREAM(get_logger(), "Autoware launch process does not respond. Kill it."); kill(process_id, SIGKILL); break; @@ -141,8 +143,11 @@ auto FieldOperatorApplication::shutdownAutoware() -> void The timeout argument specified a tv_nsec value less than zero or greater than or equal to 1000 million. */ - AUTOWARE_SYSTEM_ERROR("sigtimedwait"); - std::exit(EXIT_FAILURE); + RCLCPP_ERROR_STREAM( + get_logger(), + "The parameter sigterm_timeout specified a value less than zero or greater than or " + "equal to 1000 million."); + break; } } } From b162319e2c27e75b178bbfee8607ced2d21d69c5 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 24 Aug 2023 19:30:48 +0900 Subject: [PATCH 04/27] Cleanup Signed-off-by: yamacir-kit --- .../src/field_operator_application.cpp | 150 ++++++++---------- 1 file changed, 66 insertions(+), 84 deletions(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 83bf6d7543d..0d3cfdd2ba5 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -71,100 +71,82 @@ auto FieldOperatorApplication::checkAutowareProcess() -> void auto FieldOperatorApplication::shutdownAutoware() -> void { - AUTOWARE_INFO_STREAM("Shutting down Autoware: (1/3) Stop publishing/subscribing."); - { - stopRequest(); - } + stopRequest(); if (process_id != 0 && not is_autoware_exited) { is_autoware_exited = true; - AUTOWARE_INFO_STREAM("Shutting down Autoware: (2/3) Send SIGINT to Autoware launch process."); - { - sendSIGINT(); - } + sendSIGINT(); - AUTOWARE_INFO_STREAM("Shutting down Autoware: (2/3) Terminating Autoware."); - { - const auto sigset = [this]() { - if (auto signal_set = sigset_t(); - sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) { + const auto sigset = [this]() { + if (auto signal_set = sigset_t(); + sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) { + RCLCPP_ERROR_STREAM(get_logger(), std::system_error(errno, std::system_category()).what()); + std::exit(EXIT_FAILURE); + } else if (auto error = pthread_sigmask(SIG_BLOCK, &signal_set, nullptr)) { + RCLCPP_ERROR_STREAM(get_logger(), std::system_error(error, std::system_category()).what()); + std::exit(EXIT_FAILURE); + } else { + return signal_set; + } + }(); + + const auto timeout = [this]() { + auto sigterm_timeout = [this](auto value) { + auto node = rclcpp::Node("get_parameter_sigterm_timeout", "simulation"); + node.declare_parameter("sigterm_timeout", value); + node.get_parameter("sigterm_timeout", value); + return value; + }; + auto timeout = timespec(); + timeout.tv_sec = sigterm_timeout(5); + timeout.tv_nsec = 0; + return timeout; + }(); + + if (sigtimedwait(&sigset, nullptr, &timeout) < 0) { + switch (errno) { + case EINTR: + /* + The wait was interrupted by an unblocked, caught signal. It shall + be documented in system documentation whether this error causes + these functions to fail. + */ RCLCPP_ERROR_STREAM( - get_logger(), std::system_error(errno, std::system_category()).what()); - std::exit(EXIT_FAILURE); - } else if (auto error = pthread_sigmask(SIG_BLOCK, &signal_set, nullptr)) { + get_logger(), + "The wait for Autoware launch process termination was interrupted by an unblocked, " + "caught signal."); + break; + + case EAGAIN: + /* + No signal specified by set was generated within the specified + timeout period. + */ + RCLCPP_ERROR_STREAM(get_logger(), "Autoware launch process does not respond. Kill it."); + kill(process_id, SIGKILL); + break; + + default: + case EINVAL: + /* + The timeout argument specified a tv_nsec value less than zero or + greater than or equal to 1000 million. + */ RCLCPP_ERROR_STREAM( - get_logger(), std::system_error(error, std::system_category()).what()); - std::exit(EXIT_FAILURE); - } else { - return signal_set; - } - }(); - - const auto timeout = [this]() { - auto sigterm_timeout = [this](auto value) { - auto node = rclcpp::Node("get_parameter_sigterm_timeout", "simulation"); - node.declare_parameter("sigterm_timeout", value); - node.get_parameter("sigterm_timeout", value); - return value; - }; - auto timeout = timespec(); - timeout.tv_sec = sigterm_timeout(5); - timeout.tv_nsec = 0; - return timeout; - }(); - - if (sigtimedwait(&sigset, nullptr, &timeout) < 0) { - switch (errno) { - case EINTR: - /* - The wait was interrupted by an unblocked, caught signal. It - shall be documented in system documentation whether this error - causes these functions to fail. - */ - RCLCPP_ERROR_STREAM( - get_logger(), - "The wait for Autoware launch process termination was interrupted by an unblocked, " - "caught signal."); - break; - - case EAGAIN: - /* - No signal specified by set was generated within the specified - timeout period. - */ - RCLCPP_ERROR_STREAM(get_logger(), "Autoware launch process does not respond. Kill it."); - kill(process_id, SIGKILL); - break; - - default: - case EINVAL: - /* - The timeout argument specified a tv_nsec value less than zero or - greater than or equal to 1000 million. - */ - RCLCPP_ERROR_STREAM( - get_logger(), - "The parameter sigterm_timeout specified a value less than zero or greater than or " - "equal to 1000 million."); - break; - } + get_logger(), + "The parameter sigterm_timeout specified a value less than zero or greater than or " + "equal to 1000 million."); + break; } } - AUTOWARE_INFO_STREAM("Shutting down Autoware: (3/3) Waiting for Autoware to be exited."); - { - int status = 0; - - const int waitpid_options = 0; - - if (waitpid(process_id, &status, waitpid_options) < 0) { - if (errno == ECHILD) { - AUTOWARE_WARN_STREAM("Try to wait for the autoware process but it was already exited."); - } else { - AUTOWARE_SYSTEM_ERROR("waitpid"); - std::exit(EXIT_FAILURE); - } + if (int status = 0; waitpid(process_id, &status, 0) < 0) { + if (errno == ECHILD) { + RCLCPP_ERROR_STREAM( + get_logger(), "Try to wait for the autoware process but it was already exited."); + } else { + RCLCPP_ERROR_STREAM(get_logger(), std::system_error(errno, std::system_category()).what()); } } } From 17a9ec52ab8548fef31d2947686914e330786394 Mon Sep 17 00:00:00 2001 From: Piotr Zyskowski Date: Mon, 27 Nov 2023 15:41:09 +0100 Subject: [PATCH 05/27] 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 06/27] 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 07/27] 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 08/27] 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 09/27] 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 10/27] 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 11/27] 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 12/27] 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 13/27] 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 14/27] 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 15/27] 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 63c37281a362f21cfffa141a8731bca28bdb80f5 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 22 Jan 2024 15:45:49 +0900 Subject: [PATCH 16/27] Update to kill process group if Autoware launch process is unresponsive Signed-off-by: yamacir-kit --- external/concealer/src/field_operator_application.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 0d3cfdd2ba5..ac38fa0abec 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -124,7 +124,7 @@ auto FieldOperatorApplication::shutdownAutoware() -> void timeout period. */ RCLCPP_ERROR_STREAM(get_logger(), "Autoware launch process does not respond. Kill it."); - kill(process_id, SIGKILL); + killpg(process_id, SIGKILL); break; default: From adc1fa3fae380066588de31ff1af0984f17ca59b Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 22 Jan 2024 16:50:49 +0900 Subject: [PATCH 17/27] Lipsticks Signed-off-by: yamacir-kit --- external/concealer/src/field_operator_application.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index ac38fa0abec..b1d658b9bab 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -71,13 +71,7 @@ auto FieldOperatorApplication::checkAutowareProcess() -> void auto FieldOperatorApplication::shutdownAutoware() -> void { - stopRequest(); - - if (process_id != 0 && not is_autoware_exited) { - is_autoware_exited = true; - - sendSIGINT(); - + if (stopRequest(); process_id != 0 && not std::exchange(is_autoware_exited, true)) { const auto sigset = [this]() { if (auto signal_set = sigset_t(); sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) { @@ -104,7 +98,7 @@ auto FieldOperatorApplication::shutdownAutoware() -> void return timeout; }(); - if (sigtimedwait(&sigset, nullptr, &timeout) < 0) { + if (sendSIGINT(); sigtimedwait(&sigset, nullptr, &timeout) < 0) { switch (errno) { case EINTR: /* From f48c013c66a70ed01e4c2f9f88675a056b52f267 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 24 Jan 2024 17:32:17 +0900 Subject: [PATCH 18/27] Add new member function `StatusMonitor::overrideThreshold` Signed-off-by: yamacir-kit --- .../include/status_monitor/status_monitor.hpp | 32 +++++++++++++++++++ common/status_monitor/src/status_monitor.cpp | 4 ++- external/concealer/package.xml | 2 +- .../src/openscenario_interpreter.cpp | 20 +++++++++++- 4 files changed, 55 insertions(+), 3 deletions(-) diff --git a/common/status_monitor/include/status_monitor/status_monitor.hpp b/common/status_monitor/include/status_monitor/status_monitor.hpp index 3b8d083d542..e11373d82df 100644 --- a/common/status_monitor/include/status_monitor/status_monitor.hpp +++ b/common/status_monitor/include/status_monitor/status_monitor.hpp @@ -105,6 +105,38 @@ class StatusMonitor } } + template + class ScopedExchanger + { + std::reference_wrapper target; + + T value; + + public: + template + static auto locked_exchange(Ts &&... xs) -> decltype(auto) + { + auto lock = std::scoped_lock(mutex); + return std::exchange(std::forward(xs)...); + } + + template + explicit ScopedExchanger(T & x, U && y) + : target(x), value(locked_exchange(target.get(), std::forward(y))) + { + } + + ~ScopedExchanger() { locked_exchange(target.get(), value); } + }; + + template + auto overrideThreshold(const std::chrono::seconds & t, Thunk thunk) -> decltype(auto) + { + auto exchanger = ScopedExchanger(threshold, t); + + return thunk(); + } + auto write() const -> void; } static status_monitor; } // namespace common diff --git a/common/status_monitor/src/status_monitor.cpp b/common/status_monitor/src/status_monitor.cpp index da5879ce2f5..b00c8395bad 100644 --- a/common/status_monitor/src/status_monitor.cpp +++ b/common/status_monitor/src/status_monitor.cpp @@ -120,9 +120,11 @@ auto StatusMonitor::write() const -> void } } + json["good"] = good(); + json["name"] = name(); - json["good"] = good(); + json["threshold"] = threshold.count(); json["unix_time"] = std::chrono::duration_cast( std::chrono::high_resolution_clock::now().time_since_epoch()) diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 66da3364dd1..c64be4d513d 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -27,6 +27,7 @@ ament_index_cpp geometry_msgs + libboost-dev nav_msgs rclcpp scenario_simulator_exception @@ -34,7 +35,6 @@ tf2_geometry_msgs tf2_ros traffic_simulator_msgs - libboost-dev ament_cmake_clang_format ament_cmake_copyright diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index 919150c5655..bf5d563d88e 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #define DECLARE_PARAMETER(IDENTIFIER) \ declare_parameter(#IDENTIFIER, IDENTIFIER) @@ -293,7 +294,24 @@ auto Interpreter::reset() -> void publisher_of_context->on_deactivate(); } - SimulatorCore::deactivate(); + declare_parameter("initialize_duration", 30); + + /* + Although we have not analyzed the details yet, the process of deactivating + the simulator core takes quite a long time (especially the + traffic_simulator::API::despawnEntities function is slow). During the + process, the interpreter becomes unresponsive, which often resulted in the + status monitor thread judging the interpreter as "not good". Therefore, we + will increase the threshold of the judgment only during the process of + deactivating the simulator core. + + The threshold value here is set to the value of initialize_duration, but + there is no rationale for this; it should be larger than the original + threshold value of the status monitor and long enough for the simulator + core to be deactivated. + */ + common::status_monitor.overrideThreshold( + std::chrono::seconds(get_parameter("initialize_duration").as_int()), SimulatorCore::deactivate); scenarios.pop_front(); From 336fc67a2f85313edf1e7aa92597e095d4503176 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 25 Jan 2024 10:53:44 +0900 Subject: [PATCH 19/27] Fix to avoid redeclaring already declared parameter `initialize_duration` Signed-off-by: yamacir-kit --- .../openscenario_interpreter/src/openscenario_interpreter.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index bf5d563d88e..ee603287634 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -294,7 +294,9 @@ auto Interpreter::reset() -> void publisher_of_context->on_deactivate(); } - declare_parameter("initialize_duration", 30); + if (not has_parameter("initialize_duration")) { + declare_parameter("initialize_duration", 30); + } /* Although we have not analyzed the details yet, the process of deactivating From c0fe8ec473d7e9fca0925da97e883c857736aa3e Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 31 Jan 2024 13:02:22 +0900 Subject: [PATCH 20/27] extend matching length Signed-off-by: Masaya Kataoka --- .../ego_entity_simulation.hpp | 1 + .../ego_entity_simulation.cpp | 20 +++++++++++++++---- 2 files changed, 17 insertions(+), 4 deletions(-) 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 3d306a67f29..33c6bdea6e8 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 @@ -62,6 +62,7 @@ class EgoEntitySimulation public: const std::shared_ptr hdmap_utils_ptr_; + const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters; private: auto getCurrentPose() const -> geometry_msgs::msg::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 428ca770686..cf82916218e 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 @@ -38,7 +38,8 @@ EgoEntitySimulation::EgoEntitySimulation( : autoware(std::make_unique()), vehicle_model_type_(getVehicleModelType()), vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)), - hdmap_utils_ptr_(hdmap_utils) + hdmap_utils_ptr_(hdmap_utils), + vehicle_parameters(parameters) { } @@ -395,12 +396,23 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets()); std::optional lanelet_pose; + const auto get_matching_length = [&] { + return std::max( + vehicle_parameters.axles.front_axle.track_width, + vehicle_parameters.axles.rear_axle.track_width) * + 0.5 + + 1.0; + }; + if (unique_route_lanelets.empty()) { - lanelet_pose = hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, 1.0); + lanelet_pose = hdmap_utils_ptr_->toLaneletPose( + status.pose, status.bounding_box, false, get_matching_length()); } else { - lanelet_pose = hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, 1.0); + lanelet_pose = + hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, get_matching_length()); if (!lanelet_pose) { - lanelet_pose = hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, 1.0); + lanelet_pose = hdmap_utils_ptr_->toLaneletPose( + status.pose, status.bounding_box, false, get_matching_length()); } } if (lanelet_pose) { From 1d5de28bb08298281639972d90e3f50246405d00 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 31 Jan 2024 13:05:24 +0900 Subject: [PATCH 21/27] update release note Signed-off-by: Masaya Kataoka --- docs/ReleaseNotes.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/docs/ReleaseNotes.md b/docs/ReleaseNotes.md index f5ed96f2662..a4e83a5ef0c 100644 --- a/docs/ReleaseNotes.md +++ b/docs/ReleaseNotes.md @@ -4,8 +4,9 @@ Major Changes :race_car: :red_car: :blue_car: -| Feature | Brief summary | Category | Pull request | Contributor | -| ------- | ------------- | -------- | ------------ | ----------- | +| Feature | Brief summary | Category | Pull request | Contributor | +| ----------------------- | -------------------------------------------------------------------- | ------------------- | ----------------------------------------------------------------- | --------------------------------------------- | +| Extend matching length. | Enable consider tread when calculating matching length of EgoEntity. | `traffic_simulator` | [#1181](https://github.com/tier4/scenario_simulator_v2/pull/1181) | [hakuturu583](https://github.com/hakuturu583) | Bug Fixes:bug: From de12d608ce6987df1dcad2b61b599d494123dc06 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 2 Feb 2024 15:12:16 +0900 Subject: [PATCH 22/27] add setMapPose function Signed-off-by: Masaya Kataoka --- .../config/scenario_simulator_v2.rviz | 378 ++++++++++++++---- .../include/traffic_simulator/api/api.hpp | 1 + .../traffic_simulator/entity/ego_entity.hpp | 4 +- .../traffic_simulator/entity/entity_base.hpp | 2 + .../entity/entity_manager.hpp | 12 +- .../entity/pedestrian_entity.hpp | 2 + .../entity/vehicle_entity.hpp | 2 + simulation/traffic_simulator/src/api/api.cpp | 8 +- .../src/entity/ego_entity.cpp | 45 ++- .../src/entity/entity_base.cpp | 6 + .../src/entity/entity_manager.cpp | 12 - .../src/entity/pedestrian_entity.cpp | 1 + .../src/entity/vehicle_entity.cpp | 1 + 13 files changed, 370 insertions(+), 104 deletions(-) diff --git a/simulation/traffic_simulator/config/scenario_simulator_v2.rviz b/simulation/traffic_simulator/config/scenario_simulator_v2.rviz index 72a64ca063a..9a216c869b9 100644 --- a/simulation/traffic_simulator/config/scenario_simulator_v2.rviz +++ b/simulation/traffic_simulator/config/scenario_simulator_v2.rviz @@ -6,8 +6,9 @@ Panels: Expanded: - /Map1/Lanelet2VectorMap1 - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /TF1 Splitter Ratio: 0.557669460773468 - Tree Height: 914 + Tree Height: 1281 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -132,6 +133,11 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + ars408_front_center: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true base_link: Alpha: 1 Show Axes: false @@ -373,29 +379,11 @@ Visualization Manager: Namespaces: center_lane_line: false center_line_arrows: false - crosswalk_lanelets: true lane_start_bound: false - lanelet direction: true lanelet_id: false left_lane_bound: true - no_stopping_area: true - parking_lots: true - partitions: true - pedestrian_marking: true right_lane_bound: true road_lanelets: false - shoulder_center_lane_line: true - shoulder_center_line_arrows: true - shoulder_lane_start_bound: true - shoulder_lanelet direction: true - shoulder_left_lane_bound: true - shoulder_right_lane_bound: true - shoulder_road_lanelets: false - stop_lines: true - traffic_light: true - traffic_light_id: false - traffic_light_triangle: true - walkway_lanelets: true Topic: Depth: 5 Durability Policy: Transient Local @@ -749,14 +737,20 @@ Visualization Manager: Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% Display Acceleration: true + Display Existence Probability: false Display Label: true - Display PoseWithCovariance: true + Display Pose Covariance: true Display Predicted Path Confidence: true Display Predicted Paths: true Display Twist: true + Display Twist Covariance: false Display UUID: true Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false Enabled: true Line Width: 0.029999999329447746 MOTORCYCLE: @@ -800,14 +794,21 @@ Visualization Manager: Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/TrackedObjects + Confidence Interval: 95% Display Acceleration: true + Display Existence Probability: false Display Label: true - Display PoseWithCovariance: true + Display Pose Covariance: true Display Predicted Path Confidence: true Display Predicted Paths: true Display Twist: true + Display Twist Covariance: false Display UUID: true Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false + Dynamic Status: All Enabled: true Line Width: 0.029999999329447746 MOTORCYCLE: @@ -851,14 +852,20 @@ Visualization Manager: Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/PredictedObjects + Confidence Interval: 95% Display Acceleration: true + Display Existence Probability: false Display Label: true - Display PoseWithCovariance: false + Display Pose Covariance: true Display Predicted Path Confidence: true Display Predicted Paths: true Display Twist: true + Display Twist Covariance: false Display UUID: true Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false Enabled: true Line Width: 0.5 MOTORCYCLE: @@ -866,7 +873,15 @@ Visualization Manager: Color: 119; 11; 32 Name: PredictedObjects Namespaces: - {} + acceleration: true + label: true + path: true + path confidence: true + position covariance: true + shape: true + twist: true + uuid: true + velocity: true PEDESTRIAN: Alpha: 0.9990000128746033 Color: 255; 192; 203 @@ -969,7 +984,6 @@ Visualization Manager: Enabled: true Name: RouteArea Namespaces: - end_lanelets: true goal_lanelets: true lane_start_bound: false left_lane_bound: false @@ -1022,10 +1036,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 + Rear Overhang: 1.100000023841858 Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 + Vehicle Length: 4.889999866485596 + Vehicle Width: 1.8960000276565552 View Path: Alpha: 0.9990000128746033 Color: 0; 0; 0 @@ -1039,6 +1053,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1073,10 +1090,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 + Rear Overhang: 1.100000023841858 Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 + Vehicle Length: 4.889999866485596 + Vehicle Width: 1.8960000276565552 View Path: Alpha: 0.4000000059604645 Color: 0; 0; 0 @@ -1090,6 +1107,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1137,6 +1157,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1184,6 +1207,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1231,6 +1257,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1278,6 +1307,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1325,6 +1357,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1372,6 +1407,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1402,10 +1440,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 + Rear Overhang: 1.100000023841858 Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 + Vehicle Length: 4.889999866485596 + Vehicle Width: 1.8960000276565552 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1419,6 +1457,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1466,6 +1507,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1496,10 +1540,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 + Rear Overhang: 1.100000023841858 Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 + Vehicle Length: 4.889999866485596 + Vehicle Width: 1.8960000276565552 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1513,6 +1557,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1543,10 +1590,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 + Rear Overhang: 1.100000023841858 Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 + Vehicle Length: 4.889999866485596 + Vehicle Width: 1.8960000276565552 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1560,6 +1607,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1607,6 +1657,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1654,6 +1707,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1684,10 +1740,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 + Rear Overhang: 1.100000023841858 Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 + Vehicle Length: 4.889999866485596 + Vehicle Width: 1.8960000276565552 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1701,6 +1757,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1731,10 +1790,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 + Rear Overhang: 1.100000023841858 Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 + Vehicle Length: 4.889999866485596 + Vehicle Width: 1.8960000276565552 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1748,6 +1807,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1778,10 +1840,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 + Rear Overhang: 1.100000023841858 Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 + Vehicle Length: 4.889999866485596 + Vehicle Width: 1.8960000276565552 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1795,6 +1857,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1822,7 +1887,10 @@ Visualization Manager: Enabled: true Name: VirtualWall (Avoidance) Namespaces: - {} + dead_line_factor_text: true + dead_line_virtual_wall: true + stop_factor_text: true + stop_virtual_wall: true Topic: Depth: 5 Durability Policy: Volatile @@ -1942,8 +2010,7 @@ Visualization Manager: Enabled: true Name: VirtualWall (Walkway) Namespaces: - 177703_stop_factor_text: true - 177703_stop_virtual_wall: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1979,8 +2046,7 @@ Visualization Manager: Enabled: true Name: VirtualWall (MergeFromPrivateArea) Namespaces: - stop_factor_text: true - stop_virtual_wall: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -2553,6 +2619,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -2604,7 +2673,8 @@ Visualization Manager: Enabled: true Name: VirtualWall (ObstacleCruise) Namespaces: - {} + stop_factor_text: true + stop_virtual_wall: true Topic: Depth: 5 Durability Policy: Volatile @@ -2862,10 +2932,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 + Rear Overhang: 1.100000023841858 Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 + Vehicle Length: 4.889999866485596 + Vehicle Width: 1.8960000276565552 View Path: Alpha: 1 Color: 255; 255; 255 @@ -2879,6 +2949,9 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false + View Text Slope: + Scale: 0.30000001192092896 + Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -2944,6 +3017,7 @@ Visualization Manager: Enabled: true Name: Entity Marker Namespaces: + Npc1: true ego: true Topic: Depth: 5 @@ -2977,9 +3051,16 @@ Visualization Manager: Value: /simulation/traffic_light/marker Value: true - Class: openscenario_visualization/VisualizationConditionGroups + Enabled: true + Left: 0 + Length: 2000 Name: Condition Group + Text Color: 255; 255; 255 + Top: 450 Topic: /simulation/context Value: true + Value Scale: 35 + Width: 2000 - Class: rviz_default_plugins/MarkerArray Enabled: true Name: Debug Marker @@ -2991,7 +3072,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /simulation/debug_marker - Value: false + Value: true - Class: rviz_common/Group Displays: - Alpha: 0.9990000128746033 @@ -3038,14 +3119,20 @@ Visualization Manager: Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Confidence Interval: 95% Display Acceleration: true + Display Existence Probability: false Display Label: true - Display PoseWithCovariance: true + Display Pose Covariance: true Display Predicted Path Confidence: true Display Predicted Paths: true Display Twist: true + Display Twist Covariance: false Display UUID: true Display Velocity: true + Display Yaw Covariance: false + Display Yaw Rate: false + Display Yaw Rate Covariance: false Enabled: true Line Width: 0.029999999329447746 MOTORCYCLE: @@ -3098,8 +3185,152 @@ Visualization Manager: Value: false Enabled: true Name: Simple Sensor Simulator - Enabled: false + Enabled: true Name: Simulation + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Npc1: + Value: true + ars408_front_center: + Value: true + base_link: + Value: true + camera0/camera_link: + Value: true + camera0/camera_optical_link: + Value: true + camera1/camera_link: + Value: true + camera1/camera_optical_link: + Value: true + camera2/camera_link: + Value: true + camera2/camera_optical_link: + Value: true + camera3/camera_link: + Value: true + camera3/camera_optical_link: + Value: true + camera4/camera_link: + Value: true + camera4/camera_optical_link: + Value: true + camera5/camera_link: + Value: true + camera5/camera_optical_link: + Value: true + camera6/camera_link: + Value: true + camera6/camera_optical_link: + Value: true + camera7/camera_link: + Value: true + camera7/camera_optical_link: + Value: true + ego: + Value: true + gnss_link: + Value: true + livox_front_left: + Value: true + livox_front_left_base_link: + Value: true + livox_front_right: + Value: true + livox_front_right_base_link: + Value: true + map: + Value: true + sensor_kit_base_link: + Value: true + tamagawa/imu_link: + Value: true + velodyne_left: + Value: true + velodyne_left_base_link: + Value: true + velodyne_rear: + Value: true + velodyne_rear_base_link: + Value: true + velodyne_right: + Value: true + velodyne_right_base_link: + Value: true + velodyne_top: + Value: true + velodyne_top_base_link: + Value: true + viewer: + Value: true + Marker Scale: 10 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + Npc1: + {} + base_link: + ars408_front_center: + {} + livox_front_left_base_link: + livox_front_left: + {} + livox_front_right_base_link: + livox_front_right: + {} + sensor_kit_base_link: + camera0/camera_link: + camera0/camera_optical_link: + {} + camera1/camera_link: + camera1/camera_optical_link: + {} + camera2/camera_link: + camera2/camera_optical_link: + {} + camera3/camera_link: + camera3/camera_optical_link: + {} + camera4/camera_link: + camera4/camera_optical_link: + {} + camera5/camera_link: + camera5/camera_optical_link: + {} + camera6/camera_link: + camera6/camera_optical_link: + {} + camera7/camera_link: + camera7/camera_optical_link: + {} + gnss_link: + {} + tamagawa/imu_link: + {} + velodyne_left_base_link: + velodyne_left: + {} + velodyne_right_base_link: + velodyne_right: + {} + velodyne_top_base_link: + velodyne_top: + {} + velodyne_rear_base_link: + velodyne_rear: + {} + ego: + {} + viewer: + {} + Update Interval: 0 + Value: true Enabled: true Global Options: Background Color: 10; 10; 10 @@ -3197,21 +3428,26 @@ Visualization Manager: Value: true Views: Current: - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 278.631103515625 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 19.597293853759766 + Pitch: 0.070398710668087 Target Frame: ego - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 3.0053985118865967 Saved: - Class: rviz_default_plugins/ThirdPersonFollower Distance: 18 @@ -3255,12 +3491,12 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 2083 + Height: 2096 Hide Left Dock: false Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 RecognitionResultOnImage: collapsed: false Selection: @@ -3269,6 +3505,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 3774 - X: 2626 - Y: 41 + Width: 3770 + X: 70 + Y: 27 diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index b66ec2b09a7..9d285406ac6 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -340,6 +340,7 @@ class API FORWARD_TO_ENTITY_MANAGER(setDecelerationLimit); FORWARD_TO_ENTITY_MANAGER(setDecelerationRateLimit); FORWARD_TO_ENTITY_MANAGER(setLinearVelocity); + FORWARD_TO_ENTITY_MANAGER(setMapPose); FORWARD_TO_ENTITY_MANAGER(setVelocityLimit); FORWARD_TO_ENTITY_MANAGER(toMapPose); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index 833a0922450..c26d3d10f5f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -107,8 +107,6 @@ class EgoEntity : public VehicleEntity auto setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) -> void override; - auto setStatusExternally(const CanonicalizedEntityStatus & status) -> void; - void requestSpeedChange(double, bool continuous) override; void requestSpeedChange( @@ -116,6 +114,8 @@ class EgoEntity : public VehicleEntity auto setVelocityLimit(double) -> void override; + auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void override; + auto fillLaneletPose(CanonicalizedEntityStatus & status) -> void override; }; } // namespace entity diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 68c49c9b560..a71a056c598 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -219,6 +219,8 @@ class EntityBase virtual auto setVelocityLimit(double) -> void; + virtual auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void; + virtual void startNpcLogic(); /* */ void stopAtCurrentPosition(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index f9516e14c47..7ae0c3117c0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -282,7 +282,7 @@ class EntityManager // clang-format on FORWARD_TO_ENTITY(asFieldOperatorApplication, const); - FORWARD_TO_ENTITY(cancelRequest, ); + FORWARD_TO_ENTITY(fillLaneletPose, const); FORWARD_TO_ENTITY(get2DPolygon, const); FORWARD_TO_ENTITY(getBehaviorParameter, const); FORWARD_TO_ENTITY(getBoundingBox, const); @@ -297,27 +297,28 @@ class EntityManager FORWARD_TO_ENTITY(getDistanceToRightLaneBound, const); FORWARD_TO_ENTITY(getEntityStatusBeforeUpdate, const); FORWARD_TO_ENTITY(getEntityType, const); - FORWARD_TO_ENTITY(fillLaneletPose, const); FORWARD_TO_ENTITY(getLaneletPose, const); FORWARD_TO_ENTITY(getLinearJerk, const); FORWARD_TO_ENTITY(getMapPose, const); FORWARD_TO_ENTITY(getMapPoseFromRelativePose, const); - FORWARD_TO_ENTITY(getRouteLanelets, ); + FORWARD_TO_ENTITY(getRouteLanelets, const); FORWARD_TO_ENTITY(getStandStillDuration, const); FORWARD_TO_ENTITY(getTraveledDistance, const); FORWARD_TO_ENTITY(laneMatchingSucceed, const); + FORWARD_TO_ENTITY(activateOutOfRangeJob, ); + FORWARD_TO_ENTITY(cancelRequest, ); FORWARD_TO_ENTITY(requestAcquirePosition, ); FORWARD_TO_ENTITY(requestAssignRoute, ); FORWARD_TO_ENTITY(requestFollowTrajectory, ); FORWARD_TO_ENTITY(requestLaneChange, ); FORWARD_TO_ENTITY(requestWalkStraight, ); - FORWARD_TO_ENTITY(activateOutOfRangeJob, ); FORWARD_TO_ENTITY(setAccelerationLimit, ); FORWARD_TO_ENTITY(setAccelerationRateLimit, ); FORWARD_TO_ENTITY(setBehaviorParameter, ); FORWARD_TO_ENTITY(setDecelerationLimit, ); FORWARD_TO_ENTITY(setDecelerationRateLimit, ); FORWARD_TO_ENTITY(setLinearVelocity, ); + FORWARD_TO_ENTITY(setMapPose, ); FORWARD_TO_ENTITY(setVelocityLimit, ); #undef FORWARD_TO_ENTITY @@ -473,9 +474,6 @@ class EntityManager auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus &) -> void; - auto setEntityStatusExternally(const std::string & name, const CanonicalizedEntityStatus &) - -> void; - void setVerbose(const bool verbose); template diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp index bb6221cb03c..9c4ad92f3fe 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp @@ -119,6 +119,8 @@ class PedestrianEntity : public EntityBase const std::string plugin_name; + const traffic_simulator_msgs::msg::PedestrianParameters pedestrian_parameters; + private: pluginlib::ClassLoader loader_; const std::shared_ptr behavior_plugin_ptr_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp index 66d6e9dc71c..b7b8507e767 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -122,6 +122,8 @@ class VehicleEntity : public EntityBase auto fillLaneletPose(CanonicalizedEntityStatus & status) -> void override; + const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters; + private: pluginlib::ClassLoader loader_; diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 133147d84e1..66c575f20ff 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -237,13 +237,7 @@ bool API::updateEntitiesStatusInSim() simulation_interface::toMsg(res_status.action_status(), entity_status.action_status); if (entity_manager_ptr_->isEgo(name)) { - // temporarily deinitialize lanelet pose as it should be correctly filled from here - entity_status.lanelet_pose_valid = false; - entity_status.lanelet_pose = traffic_simulator_msgs::msg::LaneletPose(); - auto canonicalized = canonicalize(entity_status); - /// @note apply additional status data (from ll2) and then set status - entity_manager_ptr_->fillLaneletPose(name, canonicalized); - entity_manager_ptr_->setEntityStatusExternally(name, canonicalized); + setMapPose(name, entity_status.pose); } else { setEntityStatus(name, canonicalize(entity_status)); } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index b6f07378018..dc774d57a58 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -253,11 +253,6 @@ auto EgoEntity::setBehaviorParameter(const traffic_simulator_msgs::msg::Behavior { } -auto EgoEntity::setStatusExternally(const CanonicalizedEntityStatus & status) -> void -{ - externally_updated_status_ = status; -} - void EgoEntity::requestSpeedChange(double value, bool) { field_operator_application->restrictTargetSpeed(value); @@ -278,5 +273,45 @@ auto EgoEntity::fillLaneletPose(CanonicalizedEntityStatus & status) -> void EntityBase::fillLaneletPose(status, false); } +auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void +{ + const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(getRouteLanelets()); + std::optional lanelet_pose; + const auto get_matching_length = [&] { + return std::max( + vehicle_parameters.axles.front_axle.track_width, + vehicle_parameters.axles.rear_axle.track_width) * + 0.5 + + 1.0; + }; + if (unique_route_lanelets.empty()) { + lanelet_pose = + hdmap_utils_ptr_->toLaneletPose(map_pose, getBoundingBox(), false, get_matching_length()); + } else { + lanelet_pose = + hdmap_utils_ptr_->toLaneletPose(map_pose, unique_route_lanelets, get_matching_length()); + if (!lanelet_pose) { + lanelet_pose = + hdmap_utils_ptr_->toLaneletPose(map_pose, getBoundingBox(), false, get_matching_length()); + } + } + geometry_msgs::msg::Pose map_pose_z_fixed = map_pose; + auto status = static_cast(status_); + if (lanelet_pose) { + math::geometry::CatmullRomSpline spline( + hdmap_utils_ptr_->getCenterPoints(lanelet_pose->lanelet_id)); + if (const auto s_value = spline.getSValue(map_pose)) { + map_pose_z_fixed.position.z = spline.getPoint(s_value.value()).z; + } + status.pose = map_pose_z_fixed; + status.lanelet_pose_valid = true; + status.lanelet_pose = lanelet_pose.value(); + } else { + status.pose = map_pose; + status.lanelet_pose_valid = false; + status.lanelet_pose = LaneletPose(); + } + status_ = CanonicalizedEntityStatus(status, hdmap_utils_ptr_); +} } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index fff0eae220d..2e07af566dc 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -738,6 +738,12 @@ void EntityBase::setTrafficLightManager( traffic_light_manager_ = traffic_light_manager; } +auto EntityBase::setMapPose(const geometry_msgs::msg::Pose &) -> void +{ + THROW_SEMANTIC_ERROR( + "You cannot set map pose to the vehicle other than ego named ", std::quoted(name), "."); +} + void EntityBase::activateOutOfRangeJob( double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, double max_jerk) diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 0be94cd49a4..0fc5f2ecd73 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -711,18 +711,6 @@ auto EntityManager::setEntityStatus( } } -auto EntityManager::setEntityStatusExternally( - const std::string & name, const CanonicalizedEntityStatus & status) -> void -{ - if (not isEgo(name)) { - THROW_SEMANTIC_ERROR( - "You cannot set entity status externally to the vehicle other than ego named ", - std::quoted(name), "."); - } else { - dynamic_cast(entities_[name].get())->setStatusExternally(status); - } -} - void EntityManager::setVerbose(const bool verbose) { configuration.verbose = verbose; diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index 675f97af028..743726912fb 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -31,6 +31,7 @@ PedestrianEntity::PedestrianEntity( const std::string & plugin_name) : EntityBase(name, entity_status, hdmap_utils_ptr), plugin_name(plugin_name), + pedestrian_parameters(parameters), loader_(pluginlib::ClassLoader( "traffic_simulator", "entity_behavior::BehaviorPluginBase")), behavior_plugin_ptr_(loader_.createSharedInstance(plugin_name)), diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index d5b998230c6..f07e8355d23 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -31,6 +31,7 @@ VehicleEntity::VehicleEntity( const traffic_simulator_msgs::msg::VehicleParameters & parameters, const std::string & plugin_name) : EntityBase(name, entity_status, hdmap_utils_ptr), + vehicle_parameters(parameters), loader_(pluginlib::ClassLoader( "traffic_simulator", "entity_behavior::BehaviorPluginBase")), behavior_plugin_ptr_(loader_.createSharedInstance(plugin_name)), From a882dd8d3f7b4e65b00734b95b6b93f8092ba041 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 2 Feb 2024 16:01:10 +0900 Subject: [PATCH 23/27] add setTwist/setAcceleration function Signed-off-by: Masaya Kataoka --- .../sensor_simulation/sensor_simulation.hpp | 2 +- .../include/traffic_simulator/api/api.hpp | 2 ++ .../traffic_simulator/entity/entity_base.hpp | 4 ++++ .../traffic_simulator/entity/entity_manager.hpp | 2 ++ simulation/traffic_simulator/src/api/api.cpp | 2 ++ .../traffic_simulator/src/entity/entity_base.cpp | 14 ++++++++++++++ 6 files changed, 25 insertions(+), 1 deletion(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index 7c9dfb300f9..6927a364d4f 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -92,7 +92,7 @@ class SensorSimulation } auto attachPseudoTrafficLightsDetector( - const double current_simulation_time, + const double /*current_simulation_time*/, const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration, rclcpp::Node & node, std::shared_ptr hdmap_utils) -> void { diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 9d285406ac6..3c5775d385e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -333,6 +333,7 @@ class API FORWARD_TO_ENTITY_MANAGER(requestWalkStraight); FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate); FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate); + FORWARD_TO_ENTITY_MANAGER(setAcceleration); FORWARD_TO_ENTITY_MANAGER(setAccelerationLimit); FORWARD_TO_ENTITY_MANAGER(setAccelerationRateLimit); FORWARD_TO_ENTITY_MANAGER(setBehaviorParameter); @@ -341,6 +342,7 @@ class API FORWARD_TO_ENTITY_MANAGER(setDecelerationRateLimit); FORWARD_TO_ENTITY_MANAGER(setLinearVelocity); FORWARD_TO_ENTITY_MANAGER(setMapPose); + FORWARD_TO_ENTITY_MANAGER(setTwist); FORWARD_TO_ENTITY_MANAGER(setVelocityLimit); FORWARD_TO_ENTITY_MANAGER(toMapPose); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index a71a056c598..81399e55e91 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -221,6 +221,10 @@ class EntityBase virtual auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void; + /* */ auto setTwist(const geometry_msgs::msg::Twist & twist) -> void; + + /* */ auto setAcceleration(const geometry_msgs::msg::Accel & accel) -> void; + virtual void startNpcLogic(); /* */ void stopAtCurrentPosition(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 7ae0c3117c0..0359bb27b28 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -312,6 +312,7 @@ class EntityManager FORWARD_TO_ENTITY(requestFollowTrajectory, ); FORWARD_TO_ENTITY(requestLaneChange, ); FORWARD_TO_ENTITY(requestWalkStraight, ); + FORWARD_TO_ENTITY(setAcceleration, ); FORWARD_TO_ENTITY(setAccelerationLimit, ); FORWARD_TO_ENTITY(setAccelerationRateLimit, ); FORWARD_TO_ENTITY(setBehaviorParameter, ); @@ -319,6 +320,7 @@ class EntityManager FORWARD_TO_ENTITY(setDecelerationRateLimit, ); FORWARD_TO_ENTITY(setLinearVelocity, ); FORWARD_TO_ENTITY(setMapPose, ); + FORWARD_TO_ENTITY(setTwist, ); FORWARD_TO_ENTITY(setVelocityLimit, ); #undef FORWARD_TO_ENTITY diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 66c575f20ff..9f530e7007b 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -238,6 +238,8 @@ bool API::updateEntitiesStatusInSim() if (entity_manager_ptr_->isEgo(name)) { setMapPose(name, entity_status.pose); + setTwist(name, entity_status.action_status.twist); + setAcceleration(name, entity_status.action_status.accel); } else { setEntityStatus(name, canonicalize(entity_status)); } diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 2e07af566dc..f29591fb621 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -738,6 +738,20 @@ void EntityBase::setTrafficLightManager( traffic_light_manager_ = traffic_light_manager; } +auto EntityBase::setTwist(const geometry_msgs::msg::Twist & twist) -> void +{ + auto new_status = static_cast(getStatus()); + new_status.action_status.twist = twist; + status_ = CanonicalizedEntityStatus(new_status, hdmap_utils_ptr_); +} + +auto EntityBase::setAcceleration(const geometry_msgs::msg::Accel & accel) -> void +{ + auto new_status = static_cast(getStatus()); + new_status.action_status.accel = accel; + status_ = CanonicalizedEntityStatus(new_status, hdmap_utils_ptr_); +} + auto EntityBase::setMapPose(const geometry_msgs::msg::Pose &) -> void { THROW_SEMANTIC_ERROR( From 55cd97f72ae9de6b4944acf9148ef07d3937d1af Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 2 Feb 2024 16:13:29 +0900 Subject: [PATCH 24/27] remove externally_updated_status_ Signed-off-by: Masaya Kataoka --- .../include/traffic_simulator/entity/ego_entity.hpp | 2 -- simulation/traffic_simulator/src/entity/ego_entity.cpp | 4 +--- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index c26d3d10f5f..d29a5a1b330 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -39,8 +39,6 @@ class EgoEntity : public VehicleEntity static auto makeFieldOperatorApplication(const Configuration &) -> std::unique_ptr; - CanonicalizedEntityStatus externally_updated_status_; - public: explicit EgoEntity() = delete; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index dc774d57a58..a9fb5ac3fc1 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -82,8 +82,7 @@ EgoEntity::EgoEntity( const traffic_simulator_msgs::msg::VehicleParameters & parameters, const Configuration & configuration) : VehicleEntity(name, entity_status, hdmap_utils_ptr, parameters), - field_operator_application(makeFieldOperatorApplication(configuration)), - externally_updated_status_(entity_status) + field_operator_application(makeFieldOperatorApplication(configuration)) { } @@ -155,7 +154,6 @@ auto EgoEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsAr void EgoEntity::onUpdate(double current_time, double step_time) { EntityBase::onUpdate(current_time, step_time); - setStatus(externally_updated_status_); updateStandStillDuration(step_time); updateTraveledDistance(step_time); From 0f286fb71bb00c71e31f01db16305044a4508d40 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 2 Feb 2024 16:23:06 +0900 Subject: [PATCH 25/27] modify rviz settings Signed-off-by: Masaya Kataoka --- .../config/scenario_simulator_v2.rviz | 378 ++++-------------- 1 file changed, 71 insertions(+), 307 deletions(-) diff --git a/simulation/traffic_simulator/config/scenario_simulator_v2.rviz b/simulation/traffic_simulator/config/scenario_simulator_v2.rviz index 9a216c869b9..72a64ca063a 100644 --- a/simulation/traffic_simulator/config/scenario_simulator_v2.rviz +++ b/simulation/traffic_simulator/config/scenario_simulator_v2.rviz @@ -6,9 +6,8 @@ Panels: Expanded: - /Map1/Lanelet2VectorMap1 - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - - /TF1 Splitter Ratio: 0.557669460773468 - Tree Height: 1281 + Tree Height: 914 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -133,11 +132,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - ars408_front_center: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false @@ -379,11 +373,29 @@ Visualization Manager: Namespaces: center_lane_line: false center_line_arrows: false + crosswalk_lanelets: true lane_start_bound: false + lanelet direction: true lanelet_id: false left_lane_bound: true + no_stopping_area: true + parking_lots: true + partitions: true + pedestrian_marking: true right_lane_bound: true road_lanelets: false + shoulder_center_lane_line: true + shoulder_center_line_arrows: true + shoulder_lane_start_bound: true + shoulder_lanelet direction: true + shoulder_left_lane_bound: true + shoulder_right_lane_bound: true + shoulder_road_lanelets: false + stop_lines: true + traffic_light: true + traffic_light_id: false + traffic_light_triangle: true + walkway_lanelets: true Topic: Depth: 5 Durability Policy: Transient Local @@ -737,20 +749,14 @@ Visualization Manager: Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% Display Acceleration: true - Display Existence Probability: false Display Label: true - Display Pose Covariance: true + Display PoseWithCovariance: true Display Predicted Path Confidence: true Display Predicted Paths: true Display Twist: true - Display Twist Covariance: false Display UUID: true Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false Enabled: true Line Width: 0.029999999329447746 MOTORCYCLE: @@ -794,21 +800,14 @@ Visualization Manager: Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/TrackedObjects - Confidence Interval: 95% Display Acceleration: true - Display Existence Probability: false Display Label: true - Display Pose Covariance: true + Display PoseWithCovariance: true Display Predicted Path Confidence: true Display Predicted Paths: true Display Twist: true - Display Twist Covariance: false Display UUID: true Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Dynamic Status: All Enabled: true Line Width: 0.029999999329447746 MOTORCYCLE: @@ -852,20 +851,14 @@ Visualization Manager: Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/PredictedObjects - Confidence Interval: 95% Display Acceleration: true - Display Existence Probability: false Display Label: true - Display Pose Covariance: true + Display PoseWithCovariance: false Display Predicted Path Confidence: true Display Predicted Paths: true Display Twist: true - Display Twist Covariance: false Display UUID: true Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false Enabled: true Line Width: 0.5 MOTORCYCLE: @@ -873,15 +866,7 @@ Visualization Manager: Color: 119; 11; 32 Name: PredictedObjects Namespaces: - acceleration: true - label: true - path: true - path confidence: true - position covariance: true - shape: true - twist: true - uuid: true - velocity: true + {} PEDESTRIAN: Alpha: 0.9990000128746033 Color: 255; 192; 203 @@ -984,6 +969,7 @@ Visualization Manager: Enabled: true Name: RouteArea Namespaces: + end_lanelets: true goal_lanelets: true lane_start_bound: false left_lane_bound: false @@ -1036,10 +1022,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.100000023841858 + Rear Overhang: 1.0299999713897705 Value: false - Vehicle Length: 4.889999866485596 - Vehicle Width: 1.8960000276565552 + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.9990000128746033 Color: 0; 0; 0 @@ -1053,9 +1039,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1090,10 +1073,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.100000023841858 + Rear Overhang: 1.0299999713897705 Value: false - Vehicle Length: 4.889999866485596 - Vehicle Width: 1.8960000276565552 + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.4000000059604645 Color: 0; 0; 0 @@ -1107,9 +1090,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1157,9 +1137,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1207,9 +1184,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1257,9 +1231,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1307,9 +1278,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1357,9 +1325,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1407,9 +1372,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1440,10 +1402,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.100000023841858 + Rear Overhang: 1.0299999713897705 Value: false - Vehicle Length: 4.889999866485596 - Vehicle Width: 1.8960000276565552 + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1457,9 +1419,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1507,9 +1466,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1540,10 +1496,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.100000023841858 + Rear Overhang: 1.0299999713897705 Value: false - Vehicle Length: 4.889999866485596 - Vehicle Width: 1.8960000276565552 + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1557,9 +1513,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1590,10 +1543,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.100000023841858 + Rear Overhang: 1.0299999713897705 Value: false - Vehicle Length: 4.889999866485596 - Vehicle Width: 1.8960000276565552 + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1607,9 +1560,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1657,9 +1607,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1707,9 +1654,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1740,10 +1684,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.100000023841858 + Rear Overhang: 1.0299999713897705 Value: false - Vehicle Length: 4.889999866485596 - Vehicle Width: 1.8960000276565552 + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1757,9 +1701,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1790,10 +1731,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.100000023841858 + Rear Overhang: 1.0299999713897705 Value: false - Vehicle Length: 4.889999866485596 - Vehicle Width: 1.8960000276565552 + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1807,9 +1748,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1840,10 +1778,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.100000023841858 + Rear Overhang: 1.0299999713897705 Value: false - Vehicle Length: 4.889999866485596 - Vehicle Width: 1.8960000276565552 + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 0.30000001192092896 Color: 115; 210; 22 @@ -1857,9 +1795,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -1887,10 +1822,7 @@ Visualization Manager: Enabled: true Name: VirtualWall (Avoidance) Namespaces: - dead_line_factor_text: true - dead_line_virtual_wall: true - stop_factor_text: true - stop_virtual_wall: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -2010,7 +1942,8 @@ Visualization Manager: Enabled: true Name: VirtualWall (Walkway) Namespaces: - {} + 177703_stop_factor_text: true + 177703_stop_virtual_wall: true Topic: Depth: 5 Durability Policy: Volatile @@ -2046,7 +1979,8 @@ Visualization Manager: Enabled: true Name: VirtualWall (MergeFromPrivateArea) Namespaces: - {} + stop_factor_text: true + stop_virtual_wall: true Topic: Depth: 5 Durability Policy: Volatile @@ -2619,9 +2553,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -2673,8 +2604,7 @@ Visualization Manager: Enabled: true Name: VirtualWall (ObstacleCruise) Namespaces: - stop_factor_text: true - stop_virtual_wall: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -2932,10 +2862,10 @@ Visualization Manager: Alpha: 1 Color: 230; 230; 50 Offset from BaseLink: 0 - Rear Overhang: 1.100000023841858 + Rear Overhang: 1.0299999713897705 Value: false - Vehicle Length: 4.889999866485596 - Vehicle Width: 1.8960000276565552 + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 View Path: Alpha: 1 Color: 255; 255; 255 @@ -2949,9 +2879,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false View Text Velocity: Scale: 0.30000001192092896 Value: false @@ -3017,7 +2944,6 @@ Visualization Manager: Enabled: true Name: Entity Marker Namespaces: - Npc1: true ego: true Topic: Depth: 5 @@ -3051,16 +2977,9 @@ Visualization Manager: Value: /simulation/traffic_light/marker Value: true - Class: openscenario_visualization/VisualizationConditionGroups - Enabled: true - Left: 0 - Length: 2000 Name: Condition Group - Text Color: 255; 255; 255 - Top: 450 Topic: /simulation/context Value: true - Value Scale: 35 - Width: 2000 - Class: rviz_default_plugins/MarkerArray Enabled: true Name: Debug Marker @@ -3072,7 +2991,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /simulation/debug_marker - Value: true + Value: false - Class: rviz_common/Group Displays: - Alpha: 0.9990000128746033 @@ -3119,20 +3038,14 @@ Visualization Manager: Alpha: 0.9990000128746033 Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% Display Acceleration: true - Display Existence Probability: false Display Label: true - Display Pose Covariance: true + Display PoseWithCovariance: true Display Predicted Path Confidence: true Display Predicted Paths: true Display Twist: true - Display Twist Covariance: false Display UUID: true Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false Enabled: true Line Width: 0.029999999329447746 MOTORCYCLE: @@ -3185,152 +3098,8 @@ Visualization Manager: Value: false Enabled: true Name: Simple Sensor Simulator - Enabled: true + Enabled: false Name: Simulation - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - Npc1: - Value: true - ars408_front_center: - Value: true - base_link: - Value: true - camera0/camera_link: - Value: true - camera0/camera_optical_link: - Value: true - camera1/camera_link: - Value: true - camera1/camera_optical_link: - Value: true - camera2/camera_link: - Value: true - camera2/camera_optical_link: - Value: true - camera3/camera_link: - Value: true - camera3/camera_optical_link: - Value: true - camera4/camera_link: - Value: true - camera4/camera_optical_link: - Value: true - camera5/camera_link: - Value: true - camera5/camera_optical_link: - Value: true - camera6/camera_link: - Value: true - camera6/camera_optical_link: - Value: true - camera7/camera_link: - Value: true - camera7/camera_optical_link: - Value: true - ego: - Value: true - gnss_link: - Value: true - livox_front_left: - Value: true - livox_front_left_base_link: - Value: true - livox_front_right: - Value: true - livox_front_right_base_link: - Value: true - map: - Value: true - sensor_kit_base_link: - Value: true - tamagawa/imu_link: - Value: true - velodyne_left: - Value: true - velodyne_left_base_link: - Value: true - velodyne_rear: - Value: true - velodyne_rear_base_link: - Value: true - velodyne_right: - Value: true - velodyne_right_base_link: - Value: true - velodyne_top: - Value: true - velodyne_top_base_link: - Value: true - viewer: - Value: true - Marker Scale: 10 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - map: - Npc1: - {} - base_link: - ars408_front_center: - {} - livox_front_left_base_link: - livox_front_left: - {} - livox_front_right_base_link: - livox_front_right: - {} - sensor_kit_base_link: - camera0/camera_link: - camera0/camera_optical_link: - {} - camera1/camera_link: - camera1/camera_optical_link: - {} - camera2/camera_link: - camera2/camera_optical_link: - {} - camera3/camera_link: - camera3/camera_optical_link: - {} - camera4/camera_link: - camera4/camera_optical_link: - {} - camera5/camera_link: - camera5/camera_optical_link: - {} - camera6/camera_link: - camera6/camera_optical_link: - {} - camera7/camera_link: - camera7/camera_optical_link: - {} - gnss_link: - {} - tamagawa/imu_link: - {} - velodyne_left_base_link: - velodyne_left: - {} - velodyne_right_base_link: - velodyne_right: - {} - velodyne_top_base_link: - velodyne_top: - {} - velodyne_rear_base_link: - velodyne_rear: - {} - ego: - {} - viewer: - {} - Update Interval: 0 - Value: true Enabled: true Global Options: Background Color: 10; 10; 10 @@ -3428,26 +3197,21 @@ Visualization Manager: Value: true Views: Current: - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 278.631103515625 + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.070398710668087 + Scale: 19.597293853759766 Target Frame: ego - Value: ThirdPersonFollower (rviz_default_plugins) - Yaw: 3.0053985118865967 + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 Saved: - Class: rviz_default_plugins/ThirdPersonFollower Distance: 18 @@ -3491,12 +3255,12 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 2096 + Height: 2083 Hide Left Dock: false Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 RecognitionResultOnImage: collapsed: false Selection: @@ -3505,6 +3269,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 3770 - X: 70 - Y: 27 + Width: 3774 + X: 2626 + Y: 41 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 26/27] 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; From 68066dbf3aceb111f2a699e8290e700335a28bad Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 6 Feb 2024 18:29:56 +0900 Subject: [PATCH 27/27] doc: fix mkdocs.yml to delete unavailable page setting --- mkdocs.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/mkdocs.yml b/mkdocs.yml index 2d0bcd48ab2..b671ca063b5 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -64,7 +64,6 @@ nav: - Scenario Writing Tips: user_guide/ScenarioTips.md - Scenario Editor: - Overview: user_guide/scenario_editor/ScenarioEditorUserGuide.md - - Place an entity to relative position: user_guide/scenario_editor/RelativePosition.md - Scenario Test Runner: - Overview: user_guide/scenario_test_runner/ScenarioTestRunner.md - user_guide/scenario_test_runner/ScenarioFormatConversion.md