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.hpp>
25 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
26 #include <autoware_control_msgs/msg/control.hpp>
27 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
28 #include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
32 #include <concealer/publisher.hpp>
33 #include <concealer/service.hpp>
34 #include <concealer/subscriber.hpp>
35 #include <concealer/task_queue.hpp>
36 #include <concealer/visibility.hpp>
37 #include <geometry_msgs/msg/accel.hpp>
38 #include <geometry_msgs/msg/pose_stamped.hpp>
39 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
40 #include <rclcpp/rclcpp.hpp>
41 #include <tier4_external_api_msgs/msg/emergency.hpp>
42 #include <tier4_external_api_msgs/srv/engage.hpp>
43 #include <tier4_external_api_msgs/srv/set_velocity_limit.hpp>
44 #include <tier4_rtc_msgs/msg/cooperate_status_array.hpp>
45 #include <tier4_rtc_msgs/srv/auto_mode_with_module.hpp>
46 #include <tier4_rtc_msgs/srv/cooperate_commands.hpp>
47 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
48 #include <utility>
49 
50 namespace concealer
51 {
52 struct FieldOperatorApplication : public rclcpp::Node
53 {
54  pid_t process_id;
55 
56  bool initialized = false;
57 
58  std::atomic<bool> finalized = false;
59 
60  std::chrono::steady_clock::time_point time_limit;
61 
63 
65 
66  // clang-format off
67  using AutowareState = autoware_system_msgs::msg::AutowareState;
68  using Control = autoware_control_msgs::msg::Control;
69  using CooperateStatusArray = tier4_rtc_msgs::msg::CooperateStatusArray;
70  using Emergency = tier4_external_api_msgs::msg::Emergency;
71 #if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
72  using LocalizationInitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
73 #endif
74  using MrmState = autoware_adapi_v1_msgs::msg::MrmState;
75 #if __has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
76  using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState;
77 #endif
78 #if __has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>)
79  using RouteState = autoware_adapi_v1_msgs::msg::RouteState;
80 #endif
81  using TurnIndicatorsCommand = autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
82 
83  using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute;
84  using CooperateCommands = tier4_rtc_msgs::srv::CooperateCommands;
85  using Engage = tier4_external_api_msgs::srv::Engage;
86  using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization;
87  using SetRoute = autoware_adapi_v1_msgs::srv::SetRoute;
88  using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints;
89  using AutoModeWithModule = tier4_rtc_msgs::srv::AutoModeWithModule;
90  using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit;
91  using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
92 
97 #if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
99 #endif
101 #if __has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
102  Subscriber<OperationModeState> getOperationModeState;
103 #endif
105 #if __has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>)
106  Subscriber<RouteState> getRouteState;
107 #endif
109 
119  // clang-format on
120 
121  /*
122  The task queue must be deconstructed before any services, so it must be
123  the last class data member. (Class data members are constructed in
124  declaration order and deconstructed in reverse order.)
125  */
127 
128  template <typename Thunk = void (*)()>
130  const LegacyAutowareState & from_state, const LegacyAutowareState & to_state,
131  Thunk thunk = [] {})
132  {
133  thunk();
134 
135  auto not_to_be = [&](auto current_state) {
136  return from_state.value <= current_state.value and current_state.value < to_state.value;
137  };
138 
139  while (not finalized.load() and not_to_be(getLegacyAutowareState())) {
140  if (time_limit <= std::chrono::steady_clock::now()) {
141  throw common::AutowareError(
142  "Simulator waited for the Autoware state to transition to ", to_state,
143  ", but time is up. The current Autoware state is ", getLegacyAutowareState());
144  } else {
145  thunk();
146  rclcpp::GenericRate<std::chrono::steady_clock>(std::chrono::seconds(1)).sleep();
147  }
148  }
149  }
150 
151  CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t);
152 
154 
155  auto spinSome() -> void;
156 
157  auto engage() -> void;
158 
159  auto engageable() const -> bool;
160 
161  auto engaged() const -> bool;
162 
163  auto initialize(const geometry_msgs::msg::Pose &) -> void;
164 
165  [[deprecated(
166  "This function was deprecated since version 16.5.0 (released on 20250603). It will be deleted "
167  "after a half-year transition period (~20251203). Please use other overloads instead.")]] auto
168  plan(const std::vector<geometry_msgs::msg::PoseStamped> &, const bool) -> void;
169 
170 #if __has_include(<autoware_adapi_v1_msgs/msg/route_option.hpp>)
171  using RouteOption = autoware_adapi_v1_msgs::msg::RouteOption;
172 #else
173  using RouteOption = void;
174 #endif
175 
176  auto plan(
177  const geometry_msgs::msg::Pose & goal, const std::vector<geometry_msgs::msg::Pose> &,
178  const RouteOption &) -> void;
179 
180  auto plan(
181  const geometry_msgs::msg::Pose & goal,
182  const std::vector<autoware_adapi_v1_msgs::msg::RouteSegment> &, const RouteOption &) -> void;
183 
184  auto clearRoute() -> void;
185 
187 
188  auto requestAutoModeForCooperation(const std::string &, bool) -> void;
189 
190  auto sendCooperateCommand(const std::string &, const std::string &) -> void;
191 
192  auto setVelocityLimit(double) -> void;
193 
194  auto enableAutowareControl() -> void;
195 };
196 } // namespace concealer
197 
198 #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:53
Subscriber< AutowareState > getAutowareState
Definition: field_operator_application.hpp:93
tier4_rtc_msgs::srv::AutoModeWithModule AutoModeWithModule
Definition: field_operator_application.hpp:89
autoware_vehicle_msgs::msg::TurnIndicatorsCommand TurnIndicatorsCommand
Definition: field_operator_application.hpp:81
Service< SetRoutePoints > requestSetRoutePoints
Definition: field_operator_application.hpp:115
Subscriber< Emergency > getEmergencyState
Definition: field_operator_application.hpp:96
auto requestAutoModeForCooperation(const std::string &, bool) -> void
Definition: field_operator_application.cpp:472
Service< Engage > requestEngage
Definition: field_operator_application.hpp:112
autoware_control_msgs::msg::Control Control
Definition: field_operator_application.hpp:68
autoware_adapi_v1_msgs::srv::InitializeLocalization InitializeLocalization
Definition: field_operator_application.hpp:86
autoware_adapi_v1_msgs::srv::ClearRoute ClearRoute
Definition: field_operator_application.hpp:83
bool initialized
Definition: field_operator_application.hpp:56
Subscriber< Control > getCommand
Definition: field_operator_application.hpp:94
auto initialize(const geometry_msgs::msg::Pose &) -> void
Definition: field_operator_application.cpp:318
auto engaged() const -> bool
Definition: field_operator_application.cpp:312
Subscriber< CooperateStatusArray > getCooperateStatusArray
Definition: field_operator_application.hpp:95
Service< SetVelocityLimit > requestSetVelocityLimit
Definition: field_operator_application.hpp:117
auto plan(const std::vector< geometry_msgs::msg::PoseStamped > &, const bool) -> void
Definition: field_operator_application.cpp:355
tier4_rtc_msgs::msg::CooperateStatusArray CooperateStatusArray
Definition: field_operator_application.hpp:69
Subscriber< priority::PathWithLaneId > getPathWithLaneId
Definition: field_operator_application.hpp:104
pid_t process_id
Definition: field_operator_application.hpp:54
auto enableAutowareControl() -> void
Definition: field_operator_application.cpp:256
auto setVelocityLimit(double) -> void
Definition: field_operator_application.cpp:587
std::string minimum_risk_maneuver_state
Definition: field_operator_application.hpp:62
TaskQueue task_queue
Definition: field_operator_application.hpp:126
Service< SetRoute > requestSetRoute
Definition: field_operator_application.hpp:114
tier4_external_api_msgs::srv::SetVelocityLimit SetVelocityLimit
Definition: field_operator_application.hpp:90
Service< ChangeOperationMode > requestEnableAutowareControl
Definition: field_operator_application.hpp:118
auto clearRoute() -> void
Definition: field_operator_application.cpp:244
autoware_adapi_v1_msgs::msg::MrmState MrmState
Definition: field_operator_application.hpp:74
tier4_external_api_msgs::msg::Emergency Emergency
Definition: field_operator_application.hpp:70
auto sendCooperateCommand(const std::string &, const std::string &) -> void
Definition: field_operator_application.cpp:497
auto engageable() const -> bool
Definition: field_operator_application.cpp:305
~FieldOperatorApplication()
Definition: field_operator_application.cpp:169
Service< InitializeLocalization > requestInitialPose
Definition: field_operator_application.hpp:113
Subscriber< MrmState > getMrmState
Definition: field_operator_application.hpp:100
autoware_adapi_v1_msgs::srv::SetRoute SetRoute
Definition: field_operator_application.hpp:87
Service< ClearRoute > requestClearRoute
Definition: field_operator_application.hpp:110
std::chrono::steady_clock::time_point time_limit
Definition: field_operator_application.hpp:60
tier4_rtc_msgs::srv::CooperateCommands CooperateCommands
Definition: field_operator_application.hpp:84
autoware_adapi_v1_msgs::srv::SetRoutePoints SetRoutePoints
Definition: field_operator_application.hpp:88
Subscriber< TurnIndicatorsCommand > getTurnIndicatorsCommand
Definition: field_operator_application.hpp:108
auto engage() -> void
Definition: field_operator_application.cpp:264
Service< AutoModeWithModule > requestSetRtcAutoMode
Definition: field_operator_application.hpp:116
auto spinSome() -> void
Definition: field_operator_application.cpp:612
Service< CooperateCommands > requestCooperateCommands
Definition: field_operator_application.hpp:111
autoware_adapi_v1_msgs::srv::ChangeOperationMode ChangeOperationMode
Definition: field_operator_application.hpp:91
CONCEALER_PUBLIC FieldOperatorApplication(const pid_t)
Definition: field_operator_application.cpp:80
auto getLegacyAutowareState() const -> LegacyAutowareState
Definition: field_operator_application.cpp:600
tier4_external_api_msgs::srv::Engage Engage
Definition: field_operator_application.hpp:85
std::string minimum_risk_maneuver_behavior
Definition: field_operator_application.hpp:64
void RouteOption
Definition: field_operator_application.hpp:173
auto waitForAutowareStateToBe(const LegacyAutowareState &from_state, const LegacyAutowareState &to_state, Thunk thunk=[] {})
Definition: field_operator_application.hpp:129
autoware_system_msgs::msg::AutowareState AutowareState
Definition: field_operator_application.hpp:67
std::atomic< bool > finalized
Definition: field_operator_application.hpp:58
Definition: legacy_autoware_state.hpp:37
value_type value
Definition: legacy_autoware_state.hpp:49