Skip to content

Commit 4094abd

Browse files
yamacir-kityamacir-kit
authored
Merge pull request #1549 from tier4/refactor/concealer-7
Refactor/concealer 7
2 parents b19a888 + d110061 commit 4094abd

File tree

5 files changed

+337
-147
lines changed

5 files changed

+337
-147
lines changed

‎external/concealer/include/concealer/field_operator_application.hpp

Lines changed: 24 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -17,20 +17,16 @@
1717

1818
#include <sys/wait.h>
1919

20-
#if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
21-
#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
22-
#endif
23-
2420
#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
2521
#include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
2622
#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
2723
#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
2824
#include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
2925
#include <autoware_control_msgs/msg/control.hpp>
30-
#include <autoware_system_msgs/msg/autoware_state.hpp>
3126
#include <autoware_vehicle_msgs/msg/gear_command.hpp>
3227
#include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
3328
#include <concealer/autoware_universe.hpp>
29+
#include <concealer/legacy_autoware_state.hpp>
3430
#include <concealer/path_with_lane_id.hpp>
3531
#include <concealer/publisher.hpp>
3632
#include <concealer/service.hpp>
@@ -63,8 +59,6 @@ struct FieldOperatorApplication : public rclcpp::Node
6359

6460
std::chrono::steady_clock::time_point time_limit;
6561

66-
std::string autoware_state = "LAUNCHING";
67-
6862
std::string minimum_risk_maneuver_state;
6963

7064
std::string minimum_risk_maneuver_behavior;
@@ -74,8 +68,16 @@ struct FieldOperatorApplication : public rclcpp::Node
7468
using Control = autoware_control_msgs::msg::Control;
7569
using CooperateStatusArray = tier4_rtc_msgs::msg::CooperateStatusArray;
7670
using Emergency = tier4_external_api_msgs::msg::Emergency;
71+
#if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
7772
using LocalizationInitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
73+
#endif
7874
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
7981
using Trajectory = tier4_planning_msgs::msg::Trajectory;
8082
using TurnIndicatorsCommand = autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
8183

@@ -95,10 +97,16 @@ struct FieldOperatorApplication : public rclcpp::Node
9597
#if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
9698
Subscriber<LocalizationInitializationState> getLocalizationState;
9799
#endif
98-
Subscriber<MrmState> getMrmState;
99-
Subscriber<priority::PathWithLaneId> getPathWithLaneId;
100-
Subscriber<Trajectory> getTrajectory;
101-
Subscriber<TurnIndicatorsCommand> getTurnIndicatorsCommand;
100+
Subscriber<MrmState> getMrmState;
101+
#if __has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
102+
Subscriber<OperationModeState> getOperationModeState;
103+
#endif
104+
Subscriber<priority::PathWithLaneId> getPathWithLaneId;
105+
#if __has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>)
106+
Subscriber<RouteState> getRouteState;
107+
#endif
108+
Subscriber<Trajectory> getTrajectory;
109+
Subscriber<TurnIndicatorsCommand> getTurnIndicatorsCommand;
102110

103111
Service<ClearRoute> requestClearRoute;
104112
Service<CooperateCommands> requestCooperateCommands;
@@ -119,16 +127,15 @@ struct FieldOperatorApplication : public rclcpp::Node
119127

120128
template <typename Thunk = void (*)()>
121129
auto waitForAutowareStateToBe(
122-
const std::string & state, Thunk thunk = [] {})
130+
const LegacyAutowareState & state, Thunk thunk = [] {})
123131
{
124132
thunk();
125133

126-
while (not finalized.load() and autoware_state != state) {
134+
while (not finalized.load() and getLegacyAutowareState().value != state.value) {
127135
if (time_limit <= std::chrono::steady_clock::now()) {
128136
throw common::AutowareError(
129137
"Simulator waited for the Autoware state to transition to ", state,
130-
", but time is up. The current Autoware state is ",
131-
(autoware_state.empty() ? "not published yet" : autoware_state));
138+
", but time is up. The current Autoware state is ", getLegacyAutowareState());
132139
} else {
133140
thunk();
134141
rclcpp::GenericRate<std::chrono::steady_clock>(std::chrono::seconds(1)).sleep();
@@ -154,6 +161,8 @@ struct FieldOperatorApplication : public rclcpp::Node
154161

155162
auto clearRoute() -> void;
156163

164+
auto getLegacyAutowareState() const -> LegacyAutowareState;
165+
157166
auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray;
158167

159168
auto requestAutoModeForCooperation(const std::string &, bool) -> void;

‎external/concealer/include/concealer/legacy_autoware_state.hpp

Lines changed: 177 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,177 @@
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__LEGACY_AUTOWARE_STATE_HPP_
16+
#define CONCEALER__LEGACY_AUTOWARE_STATE_HPP_
17+
18+
#if __has_include(<autoware_system_msgs/msg/autoware_state.hpp>)
19+
#include <autoware_system_msgs/msg/autoware_state.hpp>
20+
#endif
21+
22+
#if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>)
23+
#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
24+
#endif
25+
26+
#if __has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
27+
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
28+
#endif
29+
30+
#if __has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>)
31+
#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
32+
#endif
33+
34+
namespace concealer
35+
{
36+
struct LegacyAutowareState
37+
{
38+
enum value_type {
39+
undefined,
40+
initializing,
41+
waiting_for_route,
42+
planning,
43+
waiting_for_engage,
44+
driving,
45+
arrived_goal,
46+
finalizing,
47+
};
48+
49+
value_type value;
50+
51+
constexpr LegacyAutowareState(value_type value) : value(value) {}
52+
53+
#if __has_include(<autoware_system_msgs/msg/autoware_state.hpp>)
54+
explicit LegacyAutowareState(const autoware_system_msgs::msg::AutowareState & autoware_state)
55+
: value([&]() {
56+
switch (autoware_state.state) {
57+
case autoware_system_msgs::msg::AutowareState::INITIALIZING:
58+
return initializing;
59+
case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE:
60+
return waiting_for_route;
61+
case autoware_system_msgs::msg::AutowareState::PLANNING:
62+
return planning;
63+
case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE:
64+
return waiting_for_engage;
65+
case autoware_system_msgs::msg::AutowareState::DRIVING:
66+
return driving;
67+
case autoware_system_msgs::msg::AutowareState::ARRIVED_GOAL:
68+
return arrived_goal;
69+
case autoware_system_msgs::msg::AutowareState::FINALIZING:
70+
return finalizing;
71+
}
72+
}())
73+
{
74+
}
75+
#endif
76+
77+
#if __has_include(<autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>) and \
78+
__has_include(<autoware_adapi_v1_msgs/msg/route_state.hpp>) and \
79+
__has_include(<autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>)
80+
explicit LegacyAutowareState(
81+
const autoware_adapi_v1_msgs::msg::LocalizationInitializationState & localization_state,
82+
const autoware_adapi_v1_msgs::msg::RouteState & route_state,
83+
const autoware_adapi_v1_msgs::msg::OperationModeState & operation_mode_state,
84+
const rclcpp::Time & now)
85+
: value([&]() {
86+
/*
87+
See
88+
- https://github.com/autowarefoundation/autoware.universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/document/autoware-state.md
89+
- https://github.com/autowarefoundation/autoware_universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/src/compatibility/autoware_state.cpp#L80-L141
90+
*/
91+
switch (localization_state.state) {
92+
case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNKNOWN:
93+
case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNINITIALIZED:
94+
case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::INITIALIZING:
95+
return initializing;
96+
97+
case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::INITIALIZED:
98+
switch (route_state.state) {
99+
case autoware_adapi_v1_msgs::msg::RouteState::UNKNOWN:
100+
return initializing;
101+
102+
case autoware_adapi_v1_msgs::msg::RouteState::ARRIVED:
103+
if (const auto duration = now - rclcpp::Time(route_state.stamp);
104+
duration.seconds() < 2.0) {
105+
return arrived_goal;
106+
}
107+
[[fallthrough]];
108+
109+
case autoware_adapi_v1_msgs::msg::RouteState::UNSET:
110+
return waiting_for_route;
111+
112+
case autoware_adapi_v1_msgs::msg::RouteState::SET:
113+
case autoware_adapi_v1_msgs::msg::RouteState::CHANGING:
114+
switch (operation_mode_state.mode) {
115+
case autoware_adapi_v1_msgs::msg::OperationModeState::UNKNOWN:
116+
return initializing;
117+
118+
case autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS:
119+
case autoware_adapi_v1_msgs::msg::OperationModeState::LOCAL:
120+
case autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE:
121+
if (operation_mode_state.is_autoware_control_enabled) {
122+
return driving;
123+
}
124+
[[fallthrough]];
125+
126+
case autoware_adapi_v1_msgs::msg::OperationModeState::STOP:
127+
return operation_mode_state.is_autonomous_mode_available ? waiting_for_engage
128+
: planning;
129+
130+
default:
131+
return undefined;
132+
}
133+
134+
default:
135+
return undefined;
136+
}
137+
138+
default:
139+
return undefined;
140+
}
141+
}())
142+
{
143+
}
144+
#endif
145+
146+
constexpr operator const char *() const
147+
{
148+
switch (value) {
149+
case initializing:
150+
return "INITIALIZING";
151+
case waiting_for_route:
152+
return "WAITING_FOR_ROUTE";
153+
case planning:
154+
return "PLANNING";
155+
case waiting_for_engage:
156+
return "WAITING_FOR_ENGAGE";
157+
case driving:
158+
return "DRIVING";
159+
case arrived_goal:
160+
return "ARRIVED_GOAL";
161+
case finalizing:
162+
return "FINALIZING";
163+
default:
164+
return "";
165+
}
166+
}
167+
168+
operator std::string() const { return operator const char *(); }
169+
170+
friend auto operator<<(std::ostream & os, const LegacyAutowareState & state) -> std::ostream &
171+
{
172+
return os << static_cast<const char *>(state);
173+
}
174+
};
175+
} // namespace concealer
176+
177+
#endif // CONCEALER__LEGACY_AUTOWARE_STATE_HPP_

0 commit comments

Comments
 (0)