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 & state, Thunk thunk = [] {})
128  {
129  thunk();
130 
131  while (not finalized.load() and getLegacyAutowareState().value != state.value) {
132  if (time_limit <= std::chrono::steady_clock::now()) {
133  throw common::AutowareError(
134  "Simulator waited for the Autoware state to transition to ", state,
135  ", but time is up. The current Autoware state is ", getLegacyAutowareState());
136  } else {
137  thunk();
138  rclcpp::GenericRate<std::chrono::steady_clock>(std::chrono::seconds(1)).sleep();
139  }
140  }
141  }
142 
143  CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t);
144 
146 
147  auto spinSome() -> void;
148 
149  auto engage() -> void;
150 
151  auto engageable() const -> bool;
152 
153  auto engaged() const -> bool;
154 
155  auto initialize(const geometry_msgs::msg::Pose &) -> void;
156 
157  auto plan(const std::vector<geometry_msgs::msg::PoseStamped> &) -> void;
158 
159  auto clearRoute() -> void;
160 
161  auto getLegacyAutowareState() const -> LegacyAutowareState;
162 
163  auto requestAutoModeForCooperation(const std::string &, bool) -> void;
164 
165  auto sendCooperateCommand(const std::string &, const std::string &) -> void;
166 
167  auto setVelocityLimit(double) -> void;
168 
169  auto enableAutowareControl() -> void;
170 };
171 } // namespace concealer
172 
173 #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:389
Service< Engage > requestEngage
Definition: field_operator_application.hpp:110
auto waitForAutowareStateToBe(const LegacyAutowareState &state, Thunk thunk=[] {})
Definition: field_operator_application.hpp:126
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:302
auto engaged() const -> bool
Definition: field_operator_application.cpp:296
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:504
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:414
auto engageable() const -> bool
Definition: field_operator_application.cpp:289
~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:333
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:529
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:517
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
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