scenario_simulator_v2 C++ API
openscenario_interpreter.hpp
Go to the documentation of this file.
1 // Copyright 2015 TIER IV, Inc. All rights reserved.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef OPENSCENARIO_INTERPRETER__OPENSCENARIO_INTERPRETER_HPP_
16 #define OPENSCENARIO_INTERPRETER__OPENSCENARIO_INTERPRETER_HPP_
17 
18 #include <boost/variant.hpp>
19 #include <chrono>
20 #include <lifecycle_msgs/msg/state.hpp>
21 #include <lifecycle_msgs/msg/transition.hpp>
22 #include <memory>
30 #include <openscenario_interpreter_msgs/msg/context.hpp>
31 #include <rclcpp/rclcpp.hpp>
32 #include <rclcpp_lifecycle/lifecycle_node.hpp>
34 #include <simple_junit/junit5.hpp>
35 #include <tier4_simulation_msgs/msg/user_defined_value.hpp>
36 #include <utility>
37 
38 #define INTERPRETER_INFO_STREAM(...) \
39  RCLCPP_INFO_STREAM(get_logger(), "\x1b[32m" << __VA_ARGS__ << "\x1b[0m")
40 
41 // NOTE: Error on simulation is not error of the interpreter; so we print error messages into
42 // INFO_STREAM.
43 #define INTERPRETER_ERROR_STREAM(...) \
44  RCLCPP_INFO_STREAM(get_logger(), "\x1b[1;31m" << __VA_ARGS__ << "\x1b[0m")
45 
47 {
48 class Interpreter : public rclcpp_lifecycle::LifecycleNode,
51 {
53  static inline constexpr std::chrono::seconds simulator_core_shutdown_threshold{30};
54 
55  using Context = openscenario_interpreter_msgs::msg::Context;
56 
57  const rclcpp_lifecycle::LifecyclePublisher<Context>::SharedPtr publisher_of_context;
58 
59  rclcpp_lifecycle::LifecyclePublisher<tier4_simulation_msgs::msg::UserDefinedValue>::SharedPtr
60  evaluate_time_publisher;
61 
62  rclcpp_lifecycle::LifecyclePublisher<tier4_simulation_msgs::msg::UserDefinedValue>::SharedPtr
63  update_time_publisher;
64 
65  rclcpp_lifecycle::LifecyclePublisher<tier4_simulation_msgs::msg::UserDefinedValue>::SharedPtr
66  output_time_publisher;
67 
68  double local_frame_rate;
69 
70  double local_real_time_factor;
71 
72  String osc_path;
73 
74  String output_directory;
75 
76  bool publish_empty_context;
77 
78  bool record;
79 
80  String record_option;
81 
82  String record_storage_id;
83 
84  std::shared_ptr<OpenScenario> script;
85 
86  std::list<std::shared_ptr<ScenarioDefinition>> scenarios;
87 
88  std::shared_ptr<rclcpp::TimerBase> timer;
89 
90  common::JUnit5 results;
91 
92  boost::variant<common::junit::Pass, common::junit::Failure, common::junit::Error> result;
93 
94  ExecutionTimer<> execution_timer;
95 
96  using Result = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
97 
98 public:
100  explicit Interpreter(const rclcpp::NodeOptions &);
101 
102  ~Interpreter() override;
103 
104  auto currentLocalFrameRate() const -> std::chrono::milliseconds;
105 
106  auto currentScenarioDefinition() const -> const std::shared_ptr<ScenarioDefinition> &;
107 
109 
110  auto on_activate(const rclcpp_lifecycle::State &) -> Result override;
111 
112  auto on_cleanup(const rclcpp_lifecycle::State &) -> Result override;
113 
114  auto on_configure(const rclcpp_lifecycle::State &) -> Result override;
115 
116  auto on_deactivate(const rclcpp_lifecycle::State &) -> Result override;
117 
118  auto on_error(const rclcpp_lifecycle::State &) -> Result override;
119 
120  auto on_shutdown(const rclcpp_lifecycle::State &) -> Result override;
121 
122  auto publishCurrentContext() const -> void;
123 
124  auto reset() -> void;
125 
126  template <typename T, typename... Ts>
127  auto set(Ts &&... xs) -> void
128  {
129  result = T(std::forward<decltype(xs)>(xs)...);
130 
131  results.name = boost::filesystem::path(osc_path).parent_path().parent_path().string();
132 
133  const auto suite_name = boost::filesystem::path(osc_path).parent_path().filename().string();
134 
135  const auto case_name = boost::filesystem::path(osc_path).stem().string();
136 
137  boost::apply_visitor(
138  overload(
139  [&](const common::junit::Pass & it) {
140  results.testsuite(suite_name).testcase(case_name).pass.push_back(it);
141  },
142  [&](const common::junit::Failure & it) {
143  results.testsuite(suite_name).testcase(case_name).failure.push_back(it);
144  },
145  [&](const common::junit::Error & it) {
146  results.testsuite(suite_name).testcase(case_name).error.push_back(it);
147  }),
148  result);
149 
150  results.write_to(
151  (boost::filesystem::path(output_directory) / "result.junit.xml").c_str(), " ");
152 
153  execution_timer.save(boost::filesystem::path(output_directory) / "execution_timer.json");
154  }
155 
156  template <typename ExceptionHandler, typename Thunk>
157  auto withExceptionHandler(ExceptionHandler && handle, Thunk && thunk) -> decltype(auto)
158  {
159  try {
160  return thunk();
161  }
162 
163  catch (const SpecialAction<EXIT_SUCCESS> & action) // from CustomCommandAction::exitSuccess
164  {
165  set<common::junit::Pass>();
166  return handle(action);
167  }
168 
169  catch (const SpecialAction<EXIT_FAILURE> & action) // from CustomCommandAction::exitFailure
170  {
171  set<common::junit::Failure>("SimulationFailure", action.what());
172  return handle(action);
173  }
174 
175  catch (const AutowareError & error) {
176  set<common::junit::Error>("AutowareError", error.what());
177  return handle(error);
178  }
179 
180  catch (const SemanticError & error) {
181  set<common::junit::Error>("SemanticError", error.what());
182  return handle(error);
183  }
184 
185  catch (const SimulationError & error) {
186  set<common::junit::Error>("SimulationError", error.what());
187  return handle(error);
188  }
189 
190  catch (const SyntaxError & error) {
191  set<common::junit::Error>("SyntaxError", error.what());
192  return handle(error);
193  }
194 
195  catch (const std::exception & error) // NOTE: MUST BE LAST OF CATCH STATEMENTS.
196  {
197  set<common::junit::Error>("InternalError", error.what());
198  return handle(error);
199  }
200 
201  catch (...) // FINAL BARRIER
202  {
203  set<common::junit::Error>("UnknownError", "An unknown exception has occurred");
204  return handle();
205  }
206  }
207 };
208 } // namespace openscenario_interpreter
209 
210 #endif // OPENSCENARIO_INTERPRETER__OPENSCENARIO_INTERPRETER_HPP_
Definition: openscenario_interpreter.hpp:51
auto on_error(const rclcpp_lifecycle::State &) -> Result override
Definition: openscenario_interpreter.cpp:310
auto currentLocalFrameRate() const -> std::chrono::milliseconds
Definition: openscenario_interpreter.cpp:72
auto on_cleanup(const rclcpp_lifecycle::State &) -> Result override
Definition: openscenario_interpreter.cpp:303
auto on_shutdown(const rclcpp_lifecycle::State &) -> Result override
Definition: openscenario_interpreter.cpp:317
auto makeCurrentConfiguration() const -> traffic_simulator::Configuration
Definition: openscenario_interpreter.cpp:82
auto publishCurrentContext() const -> void
Definition: openscenario_interpreter.cpp:327
auto withExceptionHandler(ExceptionHandler &&handle, Thunk &&thunk) -> decltype(auto)
Definition: openscenario_interpreter.hpp:157
auto on_activate(const rclcpp_lifecycle::State &) -> Result override
Definition: openscenario_interpreter.cpp:150
OPENSCENARIO_INTERPRETER_PUBLIC Interpreter(const rclcpp::NodeOptions &)
Definition: openscenario_interpreter.cpp:38
auto on_deactivate(const rclcpp_lifecycle::State &) -> Result override
Definition: openscenario_interpreter.cpp:296
auto set(Ts &&... xs) -> void
Definition: openscenario_interpreter.hpp:127
auto currentScenarioDefinition() const -> const std::shared_ptr< ScenarioDefinition > &
Definition: openscenario_interpreter.cpp:77
~Interpreter() override
Definition: openscenario_interpreter.cpp:70
auto on_configure(const rclcpp_lifecycle::State &) -> Result override
Definition: openscenario_interpreter.cpp:99
auto reset() -> void
Definition: openscenario_interpreter.cpp:345
Definition: custom_command_action.hpp:38
Definition: execution_timer.hpp:38
auto save(const boost::filesystem::path &output_file) -> void
Definition: execution_timer.hpp:58
std::string String
Definition: string.hpp:24
constexpr auto overload(Ts &&... xs) -> typename utility::overloaded< typename std::decay< Ts >::type... >
Definition: overload.hpp:51
Definition: hypot.hpp:22
Definition: junit5.hpp:25
#define OPENSCENARIO_INTERPRETER_PUBLIC
Definition: visibility.hpp:49
Definition: junit5.hpp:88
Definition: junit5.hpp:51
Definition: junit5.hpp:34
Definition: junit5.hpp:336
xs::string name
Definition: junit5.hpp:337
auto write_to(Ts &&... xs) const
Definition: junit5.hpp:381
auto testsuite(const xs::string &name) -> auto &
Definition: junit5.hpp:341
Definition: custom_command_action.hpp:32
Definition: configuration.hpp:31