scenario_simulator_v2 C++ API
field_operator_application.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 CONCEALER__AUTOWARE_USER_HPP_
16 #define CONCEALER__AUTOWARE_USER_HPP_
17 
18 #include <sys/wait.h>
19 
20 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
21 #include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
22 #include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
23 #include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
24 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
25 #include <autoware_control_msgs/msg/control.hpp>
26 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
27 #include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
31 #include <concealer/publisher.hpp>
32 #include <concealer/service.hpp>
33 #include <concealer/subscriber.hpp>
34 #include <concealer/task_queue.hpp>
35 #include <concealer/visibility.hpp>
36 #include <geometry_msgs/msg/accel.hpp>
37 #include <geometry_msgs/msg/pose_stamped.hpp>
38 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
39 #include <rclcpp/rclcpp.hpp>
40 #include <tier4_external_api_msgs/msg/emergency.hpp>
41 #include <tier4_external_api_msgs/srv/engage.hpp>
42 #include <tier4_external_api_msgs/srv/set_velocity_limit.hpp>
43 #include <tier4_rtc_msgs/msg/cooperate_status_array.hpp>
44 #include <tier4_rtc_msgs/srv/auto_mode_with_module.hpp>
45 #include <tier4_rtc_msgs/srv/cooperate_commands.hpp>
46 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
47 #include <utility>
48 
49 namespace concealer
50 {
51 struct FieldOperatorApplication : public rclcpp::Node
52 {
53  pid_t process_id;
54 
55  bool initialized = false;
56 
57  std::atomic<bool> finalized = false;
58 
59  std::chrono::steady_clock::time_point time_limit;
60 
62 
64 
65  // clang-format off
66  using AutowareState = autoware_system_msgs::msg::AutowareState;
67  using Control = autoware_control_msgs::msg::Control;
68  using CooperateStatusArray = tier4_rtc_msgs::msg::CooperateStatusArray;
69  using Emergency = tier4_external_api_msgs::msg::Emergency;
70 #if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
71  using LocalizationInitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
72 #endif
73  using MrmState = autoware_adapi_v1_msgs::msg::MrmState;
74 #if __has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
75  using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState;
76 #endif
77 #if __has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>)
78  using RouteState = autoware_adapi_v1_msgs::msg::RouteState;
79 #endif
80  using TurnIndicatorsCommand = autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
81 
82  using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute;
83  using CooperateCommands = tier4_rtc_msgs::srv::CooperateCommands;
84  using Engage = tier4_external_api_msgs::srv::Engage;
85  using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization;
86  using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints;
87  using AutoModeWithModule = tier4_rtc_msgs::srv::AutoModeWithModule;
88  using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit;
89  using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
90 
95 #if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
97 #endif
99 #if __has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
100  Subscriber<OperationModeState> getOperationModeState;
101 #endif
103 #if __has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>)
104  Subscriber<RouteState> getRouteState;
105 #endif
107 
116  // clang-format on
117 
118  /*
119  The task queue must be deconstructed before any services, so it must be
120  the last class data member. (Class data members are constructed in
121  declaration order and deconstructed in reverse order.)
122  */
124 
125  template <typename Thunk = void (*)()>
127  const LegacyAutowareState & from_state, const LegacyAutowareState & to_state,
128  Thunk thunk = [] {})
129  {
130  thunk();
131 
132  auto not_to_be = [&](auto current_state) {
133  return from_state.value <= current_state.value and current_state.value < to_state.value;
134  };
135 
136  while (not finalized.load() and not_to_be(getLegacyAutowareState())) {
137  if (time_limit <= std::chrono::steady_clock::now()) {
138  throw common::AutowareError(
139  "Simulator waited for the Autoware state to transition to ", to_state,
140  ", but time is up. The current Autoware state is ", getLegacyAutowareState());
141  } else {
142  thunk();
143  rclcpp::GenericRate<std::chrono::steady_clock>(std::chrono::seconds(1)).sleep();
144  }
145  }
146  }
147 
148  CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t);
149 
151 
152  auto spinSome() -> void;
153 
154  auto engage() -> void;
155 
156  auto engageable() const -> bool;
157 
158  auto engaged() const -> bool;
159 
160  auto initialize(const geometry_msgs::msg::Pose &) -> void;
161 
162  auto plan(const std::vector<geometry_msgs::msg::PoseStamped> &) -> void;
163 
164  auto clearRoute() -> void;
165 
166  auto getLegacyAutowareState() const -> LegacyAutowareState;
167 
168  auto requestAutoModeForCooperation(const std::string &, bool) -> void;
169 
170  auto sendCooperateCommand(const std::string &, const std::string &) -> void;
171 
172  auto setVelocityLimit(double) -> void;
173 
174  auto enableAutowareControl() -> void;
175 };
176 } // namespace concealer
177 
178 #endif // CONCEALER__AUTOWARE_USER_HPP_
Definition: task_queue.hpp:28
#define CONCEALER_PUBLIC
Definition: visibility.hpp:49
Definition: autoware_universe.hpp:40
Definition: lanelet_wrapper.hpp:40
geometry_msgs::msg::Pose Pose
Definition: lanelet_wrapper.hpp:66
geometry_msgs::msg::PoseStamped PoseStamped
Definition: lanelet_wrapper.hpp:67
std::string string
Definition: junit5.hpp:26
Autoware encountered some problem that led to a simulation failure.
Definition: exception.hpp:41
Definition: field_operator_application.hpp:52
Subscriber< AutowareState > getAutowareState
Definition: field_operator_application.hpp:91
tier4_rtc_msgs::srv::AutoModeWithModule AutoModeWithModule
Definition: field_operator_application.hpp:87
autoware_vehicle_msgs::msg::TurnIndicatorsCommand TurnIndicatorsCommand
Definition: field_operator_application.hpp:80
Service< SetRoutePoints > requestSetRoutePoints
Definition: field_operator_application.hpp:112
Subscriber< Emergency > getEmergencyState
Definition: field_operator_application.hpp:94
auto requestAutoModeForCooperation(const std::string &, bool) -> void
Definition: field_operator_application.cpp:415
Service< Engage > requestEngage
Definition: field_operator_application.hpp:110
autoware_control_msgs::msg::Control Control
Definition: field_operator_application.hpp:67
autoware_adapi_v1_msgs::srv::InitializeLocalization InitializeLocalization
Definition: field_operator_application.hpp:85
autoware_adapi_v1_msgs::srv::ClearRoute ClearRoute
Definition: field_operator_application.hpp:82
bool initialized
Definition: field_operator_application.hpp:55
Subscriber< Control > getCommand
Definition: field_operator_application.hpp:92
auto initialize(const geometry_msgs::msg::Pose &) -> void
Definition: field_operator_application.cpp:317
auto engaged() const -> bool
Definition: field_operator_application.cpp:311
Subscriber< CooperateStatusArray > getCooperateStatusArray
Definition: field_operator_application.hpp:93
Service< SetVelocityLimit > requestSetVelocityLimit
Definition: field_operator_application.hpp:114
tier4_rtc_msgs::msg::CooperateStatusArray CooperateStatusArray
Definition: field_operator_application.hpp:68
Subscriber< priority::PathWithLaneId > getPathWithLaneId
Definition: field_operator_application.hpp:102
pid_t process_id
Definition: field_operator_application.hpp:53
auto enableAutowareControl() -> void
Definition: field_operator_application.cpp:255
auto setVelocityLimit(double) -> void
Definition: field_operator_application.cpp:530
std::string minimum_risk_maneuver_state
Definition: field_operator_application.hpp:61
TaskQueue task_queue
Definition: field_operator_application.hpp:123
tier4_external_api_msgs::srv::SetVelocityLimit SetVelocityLimit
Definition: field_operator_application.hpp:88
Service< ChangeOperationMode > requestEnableAutowareControl
Definition: field_operator_application.hpp:115
auto clearRoute() -> void
Definition: field_operator_application.cpp:243
autoware_adapi_v1_msgs::msg::MrmState MrmState
Definition: field_operator_application.hpp:73
tier4_external_api_msgs::msg::Emergency Emergency
Definition: field_operator_application.hpp:69
auto sendCooperateCommand(const std::string &, const std::string &) -> void
Definition: field_operator_application.cpp:440
auto engageable() const -> bool
Definition: field_operator_application.cpp:304
~FieldOperatorApplication()
Definition: field_operator_application.cpp:168
Service< InitializeLocalization > requestInitialPose
Definition: field_operator_application.hpp:111
Subscriber< MrmState > getMrmState
Definition: field_operator_application.hpp:98
Service< ClearRoute > requestClearRoute
Definition: field_operator_application.hpp:108
auto plan(const std::vector< geometry_msgs::msg::PoseStamped > &) -> void
Definition: field_operator_application.cpp:354
std::chrono::steady_clock::time_point time_limit
Definition: field_operator_application.hpp:59
tier4_rtc_msgs::srv::CooperateCommands CooperateCommands
Definition: field_operator_application.hpp:83
autoware_adapi_v1_msgs::srv::SetRoutePoints SetRoutePoints
Definition: field_operator_application.hpp:86
Subscriber< TurnIndicatorsCommand > getTurnIndicatorsCommand
Definition: field_operator_application.hpp:106
auto engage() -> void
Definition: field_operator_application.cpp:263
Service< AutoModeWithModule > requestSetRtcAutoMode
Definition: field_operator_application.hpp:113
auto spinSome() -> void
Definition: field_operator_application.cpp:555
Service< CooperateCommands > requestCooperateCommands
Definition: field_operator_application.hpp:109
autoware_adapi_v1_msgs::srv::ChangeOperationMode ChangeOperationMode
Definition: field_operator_application.hpp:89
CONCEALER_PUBLIC FieldOperatorApplication(const pid_t)
Definition: field_operator_application.cpp:80
auto getLegacyAutowareState() const -> LegacyAutowareState
Definition: field_operator_application.cpp:543
tier4_external_api_msgs::srv::Engage Engage
Definition: field_operator_application.hpp:84
std::string minimum_risk_maneuver_behavior
Definition: field_operator_application.hpp:63
auto waitForAutowareStateToBe(const LegacyAutowareState &from_state, const LegacyAutowareState &to_state, Thunk thunk=[] {})
Definition: field_operator_application.hpp:126
autoware_system_msgs::msg::AutowareState AutowareState
Definition: field_operator_application.hpp:66
std::atomic< bool > finalized
Definition: field_operator_application.hpp:57
Definition: legacy_autoware_state.hpp:37
value_type value
Definition: legacy_autoware_state.hpp:49