Add execution time metrics

openscenario/openscenario_interpreter/include/openscenario_interpreter/openscenario_interpreter.hpp

@@ -32,6 +32,7 @@
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 #include <simple_junit/junit5.hpp>
+#include <tier4_simulation_msgs/msg/user_defined_value.hpp>
 #include <utility>
 
 #define INTERPRETER_INFO_STREAM(...) \
@@ -52,6 +53,15 @@ class Interpreter : public rclcpp_lifecycle::LifecycleNode,
 
   const rclcpp_lifecycle::LifecyclePublisher<Context>::SharedPtr publisher_of_context;
 
+  rclcpp_lifecycle::LifecyclePublisher<tier4_simulation_msgs::msg::UserDefinedValue>::SharedPtr
+    evaluate_time_publisher;
+
+  rclcpp_lifecycle::LifecyclePublisher<tier4_simulation_msgs::msg::UserDefinedValue>::SharedPtr
+    update_time_publisher;
+
+  rclcpp_lifecycle::LifecyclePublisher<tier4_simulation_msgs::msg::UserDefinedValue>::SharedPtr
+    output_time_publisher;
+
   double local_frame_rate;
 
   double local_real_time_factor;

openscenario/openscenario_interpreter/include/openscenario_interpreter/utility/scoped_elapsed_time_recorder.hpp

@@ -0,0 +1,38 @@
+// Copyright 2015 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 OPENSCENARIO_INTERPRETER__UTILITY__SCOPED_ELAPSED_TIME_RECORDER_HPP_
+#define OPENSCENARIO_INTERPRETER__UTILITY__SCOPED_ELAPSED_TIME_RECORDER_HPP_
+
+#include <chrono>
+#include <cmath>
+
+template <typename TClock = std::chrono::high_resolution_clock>
+class ScopedElapsedTimeRecorder
+{
+public:
+  explicit ScopedElapsedTimeRecorder(double & output_seconds) : output_seconds(output_seconds) {}
+
+  ~ScopedElapsedTimeRecorder()
+  {
+    output_seconds = std::abs(std::chrono::duration<double>(TClock::now() - start).count());
+  }
+
+private:
+  std::chrono::time_point<TClock> start = TClock::now();
+
+  double & output_seconds;
+};
+
+#endif  // OPENSCENARIO_INTERPRETER__UTILITY__SCOPED_ELAPSED_TIME_RECORDER_HPP_

openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp

@@ -24,6 +24,7 @@
 #include <openscenario_interpreter/syntax/scenario_object.hpp>
 #include <openscenario_interpreter/syntax/speed_condition.hpp>
 #include <openscenario_interpreter/utility/overload.hpp>
+#include <openscenario_interpreter/utility/scoped_elapsed_time_recorder.hpp>
 #include <status_monitor/status_monitor.hpp>
 #include <traffic_simulator/data_type/lanelet_pose.hpp>
 
@@ -37,6 +38,12 @@ namespace openscenario_interpreter
 Interpreter::Interpreter(const rclcpp::NodeOptions & options)
 : rclcpp_lifecycle::LifecycleNode("openscenario_interpreter", options),
   publisher_of_context(create_publisher<Context>("context", rclcpp::QoS(1).transient_local())),
+  evaluate_time_publisher(create_publisher<tier4_simulation_msgs::msg::UserDefinedValue>(
+    "/simulation/interpreter/execution_time_ms/evaluate", rclcpp::QoS(1).transient_local())),
+  update_time_publisher(create_publisher<tier4_simulation_msgs::msg::UserDefinedValue>(
+    "/simulation/interpreter/execution_time_ms/update", rclcpp::QoS(1).transient_local())),
+  output_time_publisher(create_publisher<tier4_simulation_msgs::msg::UserDefinedValue>(
+    "/simulation/interpreter/execution_time_ms/output", rclcpp::QoS(1).transient_local())),
   local_frame_rate(30),
   local_real_time_factor(1.0),
   osc_path(""),
@@ -186,23 +193,40 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result
       },
       [this]() {
         withTimeoutHandler(defaultTimeoutHandler(), [this]() {
-          if (std::isnan(evaluateSimulationTime())) {
-            if (not waiting_for_engagement_to_be_completed and engageable()) {
-              engage();
-              waiting_for_engagement_to_be_completed = true;  // NOTE: DIRTY HACK!!!
-            } else if (engaged()) {
-              activateNonUserDefinedControllers();
-              waiting_for_engagement_to_be_completed = false;  // NOTE: DIRTY HACK!!!
+          double evaluate_time, update_time, context_time;
+          {
+            ScopedElapsedTimeRecorder evaluate_time_recorder(evaluate_time);
+            if (std::isnan(evaluateSimulationTime())) {
+              if (not waiting_for_engagement_to_be_completed and engageable()) {
+                engage();
+                waiting_for_engagement_to_be_completed = true;  // NOTE: DIRTY HACK!!!
+              } else if (engaged()) {
+                activateNonUserDefinedControllers();
+                waiting_for_engagement_to_be_completed = false;  // NOTE: DIRTY HACK!!!
+              }
+            } else if (currentScenarioDefinition()) {
+              currentScenarioDefinition()->evaluate();
+            } else {
+              throw Error("No script evaluable.");
             }
-          } else if (currentScenarioDefinition()) {
-            currentScenarioDefinition()->evaluate();
-          } else {
-            throw Error("No script evaluable.");
+          }
+          {
+            ScopedElapsedTimeRecorder update_time_recorder(update_time);
+            SimulatorCore::update();
+          }
+          {
+            ScopedElapsedTimeRecorder context_time_recorder(context_time);
+            publishCurrentContext();
           }
 
-          SimulatorCore::update();
-
-          publishCurrentContext();
+          tier4_simulation_msgs::msg::UserDefinedValue msg;
+          msg.type.data = tier4_simulation_msgs::msg::UserDefinedValueType::DOUBLE;
+          msg.value = std::to_string(evaluate_time * 1e3);
+          evaluate_time_publisher->publish(msg);
+          msg.value = std::to_string(update_time * 1e3);
+          update_time_publisher->publish(msg);
+          msg.value = std::to_string(context_time * 1e3);
+          output_time_publisher->publish(msg);
         });
       });
   };
@@ -240,6 +264,9 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result
         execution_timer.clear();
 
         publisher_of_context->on_activate();
+        evaluate_time_publisher->on_activate();
+        update_time_publisher->on_activate();
+        output_time_publisher->on_activate();
 
         assert(publisher_of_context->is_activated());
 
@@ -311,6 +338,15 @@ auto Interpreter::reset() -> void
   if (publisher_of_context->is_activated()) {
     publisher_of_context->on_deactivate();
   }
+  if (evaluate_time_publisher->is_activated()) {
+    evaluate_time_publisher->on_deactivate();
+  }
+  if (update_time_publisher->is_activated()) {
+    update_time_publisher->on_deactivate();
+  }
+  if (output_time_publisher->is_activated()) {
+    output_time_publisher->on_deactivate();
+  }
 
   if (not has_parameter("initialize_duration")) {
     declare_parameter<int>("initialize_duration", 30);

test_runner/scenario_test_runner/config/workflow.txt

@@ -9,6 +9,7 @@ $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityConditio
 $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeSpeedCondition.yaml
 $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml
 $(find-pkg-share scenario_test_runner)/scenario/ControllerAction.AssignControllerAction.yaml
+$(find-pkg-share scenario_test_runner)/scenario/execution_time_test.yaml
 $(find-pkg-share scenario_test_runner)/scenario/LateralAction.LaneChangeAction-RoadShoulder.yaml
 $(find-pkg-share scenario_test_runner)/scenario/LongitudinalAction.SpeedAction.yaml
 $(find-pkg-share scenario_test_runner)/scenario/Property.isBlind.yaml

test_runner/scenario_test_runner/scenario/execution_time_test.yaml

@@ -0,0 +1,201 @@
+OpenSCENARIO:
+  FileHeader:
+    author: 'Kotaro Yoshimoto'
+    date: '2025-01-28T18:06:53+09:00'
+    description: 'scenario for execution time test'
+    revMajor: 1
+    revMinor: 3
+  ParameterDeclarations:
+    ParameterDeclaration: []
+  CatalogLocations:
+    VehicleCatalog:
+      Directory:
+        path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle
+  RoadNetwork:
+    LogicFile:
+      filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map
+  Entities:
+    ScenarioObject:
+      - name: Car1
+        CatalogReference: &SAMPLE_VEHICLE
+          catalogName: sample_vehicle
+          entryName: sample_vehicle
+      - name: Car2
+        CatalogReference: *SAMPLE_VEHICLE
+      - name: Car3
+        CatalogReference: *SAMPLE_VEHICLE
+      - name: Car4
+        CatalogReference: *SAMPLE_VEHICLE
+      - name: Car5
+        CatalogReference: *SAMPLE_VEHICLE
+      - name: Car6
+        CatalogReference: *SAMPLE_VEHICLE
+  Storyboard:
+    Init:
+      Actions:
+        Private:
+          - entityRef: Car1
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition: &DEFAULT_LANE_POSITION
+                      roadId: ''
+                      laneId: 34513
+                      s: 0
+                      offset: 0
+                      Orientation: &DEFAULT_ORIENTATION
+                        type: relative
+                        h: 0
+                        p: 0
+                        r: 0
+          - entityRef: Car2
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition:
+                      <<: *DEFAULT_LANE_POSITION
+                      s: 5
+          - entityRef: Car3
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition:
+                      <<: *DEFAULT_LANE_POSITION
+                      s: 10
+          - entityRef: Car4
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition:
+                      <<: *DEFAULT_LANE_POSITION
+                      s: 15
+          - entityRef: Car5
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition:
+                      <<: *DEFAULT_LANE_POSITION
+                      s: 20
+          - entityRef: Car6
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition:
+                      <<: *DEFAULT_LANE_POSITION
+                      s: 25
+    Story:
+      - name: ''
+        Act:
+          - name: _EndCondition
+            ManeuverGroup:
+              - maximumExecutionCount: 1
+                name: ''
+                Actors:
+                  selectTriggeringEntities: false
+                  EntityRef:
+                    - entityRef: Car1
+                Maneuver:
+                  - name: ''
+                    Event:
+                      - name: ''
+                        priority: parallel
+                        StartTrigger:
+                          ConditionGroup:
+                            - Condition:
+                                - name: ''
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByEntityCondition:
+                                    TriggeringEntities:
+                                      triggeringEntitiesRule: any
+                                      EntityRef:
+                                        - entityRef: Car1
+                                    EntityCondition:
+                                      ReachPositionCondition:
+                                        Position:
+                                          LanePosition:
+                                            <<: *DEFAULT_LANE_POSITION
+                                            s: 40
+                                        tolerance: 0.5
+                        Action:
+                          - name: ''
+                            UserDefinedAction:
+                              CustomCommandAction:
+                                type: exitSuccess
+                      - name: ''
+                        priority: parallel
+                        StartTrigger:
+                          ConditionGroup:
+                            - Condition:
+                                - name: ''
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByValueCondition:
+                                    SimulationTimeCondition:
+                                      value: 60
+                                      rule: greaterThan
+                            - Condition:
+                                - name: 'evaluate time checker'
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByValueCondition:
+                                    UserDefinedValueCondition:
+                                      name: /simulation/interpreter/execution_time_ms/evaluate
+                                      rule: greaterThan
+                                      value: 1
+                                - name: 'avoid startup'
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByValueCondition:
+                                    SimulationTimeCondition:
+                                      value: 1
+                                      rule: greaterThan
+                            - Condition:
+                                - name: 'update time checker'
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByValueCondition:
+                                    UserDefinedValueCondition:
+                                      name: /simulation/interpreter/execution_time_ms/update
+                                      rule: greaterThan
+                                      value: 5
+                                - name: 'avoid startup'
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByValueCondition:
+                                    SimulationTimeCondition:
+                                      value: 1
+                                      rule: greaterThan
+                            - Condition:
+                                - name: 'output time checker'
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByValueCondition:
+                                    UserDefinedValueCondition:
+                                      name: /simulation/interpreter/execution_time_ms/output
+                                      rule: greaterThan
+                                      value: 1
+                                - name: 'avoid startup'
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByValueCondition:
+                                    SimulationTimeCondition:
+                                      value: 1
+                                      rule: greaterThan
+                        Action:
+                          - name: ''
+                            UserDefinedAction:
+                              CustomCommandAction:
+                                type: exitFailure
+            StartTrigger:
+              ConditionGroup:
+                - Condition:
+                    - name: ''
+                      delay: 0
+                      conditionEdge: none
+                      ByValueCondition:
+                        SimulationTimeCondition:
+                          value: 0
+                          rule: greaterThan
+    StopTrigger:
+      ConditionGroup: []