Skip to content

Commit 2370e98

Browse files
HansRoboHansRobo
authored
Merge pull request #1590 from tier4/feature/change_allow_goal_modification
2 parents 958ed1b + b464a78 commit 2370e98

File tree

16 files changed

+601
-58
lines changed

16 files changed

+601
-58
lines changed

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -159,7 +159,7 @@ struct FieldOperatorApplication : public rclcpp::Node
159159

160160
auto initialize(const geometry_msgs::msg::Pose &) -> void;
161161

162-
auto plan(const std::vector<geometry_msgs::msg::PoseStamped> &) -> void;
162+
auto plan(const std::vector<geometry_msgs::msg::PoseStamped> &, const bool) -> void;
163163

164164
auto clearRoute() -> void;
165165

‎external/concealer/src/field_operator_application.cpp

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -351,12 +351,13 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi
351351
}
352352
}
353353

354-
auto FieldOperatorApplication::plan(const std::vector<geometry_msgs::msg::PoseStamped> & route)
354+
auto FieldOperatorApplication::plan(
355+
const std::vector<geometry_msgs::msg::PoseStamped> & route, const bool allow_goal_modification)
355356
-> void
356357
{
357358
assert(not route.empty());
358359

359-
task_queue.delay([this, route] {
360+
task_queue.delay([this, route, allow_goal_modification]() {
360361
switch (const auto state = getLegacyAutowareState(); state.value) {
361362
default:
362363
throw common::AutowareError(
@@ -396,8 +397,7 @@ auto FieldOperatorApplication::plan(const std::vector<geometry_msgs::msg::PoseSt
396397
DetectMember_option<SetRoutePoints::Request>::value and
397398
DetectMember_allow_goal_modification<
398399
decltype(std::declval<SetRoutePoints::Request>().option)>::value) {
399-
request->option.allow_goal_modification = common::getParameter<bool>(
400-
get_node_parameters_interface(), "allow_goal_modification");
400+
request->option.allow_goal_modification = allow_goal_modification;
401401
}
402402

403403
return request;

‎openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -439,9 +439,11 @@ class SimulatorCore
439439

440440
auto & ego_entity = core->getEgoEntity(entity_ref);
441441

442-
ego_entity.setParameter<bool>(
443-
"allow_goal_modification",
444-
controller.properties.template get<Boolean>("allowGoalModification"));
442+
if (controller.properties.contains("allowGoalModification")) {
443+
ego_entity.setParameter<bool>(
444+
"allow_goal_modification",
445+
controller.properties.template get<Boolean>("allowGoalModification"));
446+
}
445447

446448
for (const auto & module :
447449
[](std::string manual_modules_string) {

‎openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/properties.hpp

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,11 @@ struct Properties
6060

6161
explicit Properties(const pugi::xml_node &, Scope &);
6262

63+
auto contains(const String & name) const -> bool
64+
{
65+
return properties.find(name) != std::end(properties);
66+
}
67+
6368
template <typename T>
6469
auto get(const String & name, const T & default_value) const -> auto
6570
{

‎openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp

Lines changed: 58 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,9 @@
1515
#include <openscenario_interpreter/reader/element.hpp>
1616
#include <openscenario_interpreter/simulator_core.hpp>
1717
#include <openscenario_interpreter/syntax/acquire_position_action.hpp>
18+
#include <openscenario_interpreter/syntax/controller.hpp>
19+
#include <openscenario_interpreter/syntax/entities.hpp>
20+
#include <openscenario_interpreter/syntax/scenario_object.hpp>
1821
#include <openscenario_interpreter/utility/overload.hpp>
1922

2023
namespace openscenario_interpreter
@@ -29,21 +32,67 @@ AcquirePositionAction::AcquirePositionAction(const pugi::xml_node & node, Scope
2932
auto AcquirePositionAction::start() -> void
3033
{
3134
const auto acquire_position = overload(
32-
[](const WorldPosition & position, auto && actor) {
33-
return applyAcquirePositionAction(actor, static_cast<NativeWorldPosition>(position));
35+
[](
36+
const WorldPosition & position, auto && actor,
37+
const traffic_simulator::RouteOption & options) {
38+
return applyAcquirePositionAction(actor, static_cast<NativeWorldPosition>(position), options);
3439
},
35-
[](const RelativeWorldPosition & position, auto && actor) {
36-
return applyAcquirePositionAction(actor, static_cast<NativeLanePosition>(position));
40+
[](
41+
const RelativeWorldPosition & position, auto && actor,
42+
const traffic_simulator::RouteOption & options) {
43+
return applyAcquirePositionAction(actor, static_cast<NativeLanePosition>(position), options);
3744
},
38-
[](const RelativeObjectPosition & position, auto && actor) {
39-
return applyAcquirePositionAction(actor, static_cast<NativeLanePosition>(position));
45+
[](
46+
const RelativeObjectPosition & position, auto && actor,
47+
const traffic_simulator::RouteOption & options) {
48+
return applyAcquirePositionAction(actor, static_cast<NativeLanePosition>(position), options);
4049
},
41-
[](const LanePosition & position, auto && actor) {
42-
return applyAcquirePositionAction(actor, static_cast<NativeLanePosition>(position));
50+
[](
51+
const LanePosition & position, auto && actor,
52+
const traffic_simulator::RouteOption & options) {
53+
return applyAcquirePositionAction(actor, static_cast<NativeLanePosition>(position), options);
4354
});
4455

56+
auto allow_goal_modification_from_parameter = [&]() -> bool {
57+
try {
58+
return ref<Boolean>(std::string("AcquirePositionAction.allow_goal_modification"));
59+
} catch (const SyntaxError &) {
60+
// default value of allow_goal_modification
61+
return false;
62+
}
63+
}();
64+
65+
auto get_allow_goal_modification_from_property =
66+
[this](const EntityRef & entity_ref) -> std::optional<bool> {
67+
if (const auto & entity = global().entities->ref(entity_ref);
68+
entity.template is<ScenarioObject>()) {
69+
const auto & object_controller = entity.as<ScenarioObject>().object_controller;
70+
if (object_controller.isAutoware() && object_controller.is<Controller>()) {
71+
auto controller = object_controller.as<Controller>();
72+
if (controller.properties.contains("allowGoalModification")) {
73+
return controller.properties.template get<Boolean>("allowGoalModification");
74+
}
75+
}
76+
}
77+
return std::nullopt;
78+
};
79+
4580
for (const auto & actor : actors) {
46-
actor.apply([&](const auto & object) { apply<void>(acquire_position, position, object); });
81+
actor.apply([&](const auto & object) {
82+
traffic_simulator::RouteOption options;
83+
options.allow_goal_modification = [&]() {
84+
if (auto allow_goal_modification_from_property =
85+
get_allow_goal_modification_from_property(object);
86+
allow_goal_modification_from_property.has_value()) {
87+
// property specification takes precedence
88+
return allow_goal_modification_from_property.value();
89+
} else {
90+
return allow_goal_modification_from_parameter;
91+
}
92+
}();
93+
94+
apply<void>(acquire_position, position, object, options);
95+
});
4796
}
4897
}
4998
} // namespace syntax

‎simulation/traffic_simulator/include/traffic_simulator/data_type/route_option.hpp

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
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 TRAFFIC_SIMULATOR__DATA_TYPE__ROUTE_OPTION_HPP_
16+
#define TRAFFIC_SIMULATOR__DATA_TYPE__ROUTE_OPTION_HPP_
17+
18+
namespace traffic_simulator
19+
{
20+
inline namespace route_option
21+
{
22+
inline namespace v1
23+
{
24+
struct RouteOption
25+
{
26+
bool allow_goal_modification = false;
27+
};
28+
} // namespace v1
29+
} // namespace route_option
30+
} // namespace traffic_simulator
31+
#endif // TRAFFIC_SIMULATOR__DATA_TYPE__ROUTE_OPTION_HPP_

‎simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -82,12 +82,22 @@ class EgoEntity : public VehicleEntity, private concealer::FieldOperatorApplicat
8282

8383
void requestAcquirePosition(const CanonicalizedLaneletPose &) override;
8484

85-
void requestAcquirePosition(const geometry_msgs::msg::Pose & map_pose) override;
85+
void requestAcquirePosition(const CanonicalizedLaneletPose &, const RouteOption &) override;
86+
87+
void requestAcquirePosition(const geometry_msgs::msg::Pose &) override;
88+
89+
void requestAcquirePosition(const geometry_msgs::msg::Pose &, const RouteOption &) override;
8690

8791
void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> &) override;
8892

93+
void requestAssignRoute(
94+
const std::vector<CanonicalizedLaneletPose> &, const RouteOption &) override;
95+
8996
void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) override;
9097

98+
void requestAssignRoute(
99+
const std::vector<geometry_msgs::msg::Pose> &, const RouteOption &) override;
100+
91101
auto requestFollowTrajectory(
92102
const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void override;
93103

@@ -105,7 +115,9 @@ class EgoEntity : public VehicleEntity, private concealer::FieldOperatorApplicat
105115

106116
auto requestClearRoute() -> void;
107117

108-
auto requestReplanRoute(const std::vector<geometry_msgs::msg::PoseStamped> & route) -> void;
118+
auto requestReplanRoute(
119+
const std::vector<geometry_msgs::msg::PoseStamped> & route,
120+
const bool allow_goal_modification = false) -> void;
109121

110122
auto requestAutoModeForCooperation(const std::string & module_name, bool enable) -> void;
111123

‎simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp

Lines changed: 43 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
#include <traffic_simulator/behavior/longitudinal_speed_planning.hpp>
2727
#include <traffic_simulator/data_type/entity_status.hpp>
2828
#include <traffic_simulator/data_type/lane_change.hpp>
29+
#include <traffic_simulator/data_type/route_option.hpp>
2930
#include <traffic_simulator/data_type/speed_change.hpp>
3031
#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
3132
#include <traffic_simulator/helper/helper.hpp>
@@ -171,13 +172,51 @@ class EntityBase : public std::enable_shared_from_this<EntityBase>
171172

172173
/* */ void resetDynamicConstraints();
173174

174-
virtual void requestAcquirePosition(const CanonicalizedLaneletPose &) = 0;
175+
[[deprecated(
176+
"This function was deprecated since version 16.4.0 (released on 20250522). It will be deleted "
177+
"after a half-year transition period (~20251122). Please use one with RouteOption argument "
178+
"instead.")]] virtual void
179+
requestAcquirePosition(const CanonicalizedLaneletPose & pose)
180+
{
181+
return requestAcquirePosition(pose, {});
182+
}
183+
184+
virtual void requestAcquirePosition(const CanonicalizedLaneletPose &, const RouteOption &) = 0;
185+
186+
[[deprecated(
187+
"This function was deprecated since version 16.4.0 (released on 20250522). It will be deleted "
188+
"after a half-year transition period (~20251122). Please use one with RouteOption argument "
189+
"instead.")]] virtual void
190+
requestAcquirePosition(const geometry_msgs::msg::Pose & pose)
191+
{
192+
return requestAcquirePosition(pose, {});
193+
}
175194

176-
virtual void requestAcquirePosition(const geometry_msgs::msg::Pose &) = 0;
195+
virtual void requestAcquirePosition(const geometry_msgs::msg::Pose &, const RouteOption &) = 0;
177196

178-
virtual void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> &) = 0;
197+
[[deprecated(
198+
"This function was deprecated since version 16.4.0 (released on 20250522). It will be deleted "
199+
"after a half-year transition period (~20251122). Please use one with RouteOption argument "
200+
"instead.")]] virtual void
201+
requestAssignRoute(const std::vector<CanonicalizedLaneletPose> & pose)
202+
{
203+
return requestAssignRoute(pose, {});
204+
}
205+
206+
virtual void requestAssignRoute(
207+
const std::vector<CanonicalizedLaneletPose> &, const RouteOption &) = 0;
208+
209+
[[deprecated(
210+
"This function was deprecated since version 16.4.0 (released on 20250522). It will be deleted "
211+
"after a half-year transition period (~20251122). Please use one with RouteOption argument "
212+
"instead.")]] virtual void
213+
requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> & pose)
214+
{
215+
return requestAssignRoute(pose, {});
216+
}
179217

180-
virtual void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) = 0;
218+
virtual void requestAssignRoute(
219+
const std::vector<geometry_msgs::msg::Pose> &, const RouteOption &) = 0;
181220

182221
virtual auto requestLaneChange(const lanelet::Id) -> void
183222
{

‎simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -67,22 +67,24 @@ class MiscObjectEntity : public EntityBase
6767

6868
void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool) override;
6969

70-
void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> &) override
70+
void requestAssignRoute(
71+
const std::vector<CanonicalizedLaneletPose> &, const RouteOption &) override
7172
{
7273
THROW_SEMANTIC_ERROR("requestAssignRoute function cannot not use in MiscObjectEntity");
7374
}
7475

75-
void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) override
76+
void requestAssignRoute(
77+
const std::vector<geometry_msgs::msg::Pose> &, const RouteOption &) override
7678
{
7779
THROW_SEMANTIC_ERROR("requestAssignRoute function cannot not use in MiscObjectEntity");
7880
}
7981

80-
void requestAcquirePosition(const CanonicalizedLaneletPose &) override
82+
void requestAcquirePosition(const CanonicalizedLaneletPose &, const RouteOption &) override
8183
{
8284
THROW_SEMANTIC_ERROR("requestAcquirePosition function cannot not use in MiscObjectEntity");
8385
}
8486

85-
void requestAcquirePosition(const geometry_msgs::msg::Pose &) override
87+
void requestAcquirePosition(const geometry_msgs::msg::Pose &, const RouteOption &) override
8688
{
8789
THROW_SEMANTIC_ERROR("requestAcquirePosition function cannot not use in MiscObjectEntity");
8890
}

‎simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -70,9 +70,11 @@ class PedestrianEntity : public EntityBase
7070

7171
auto onUpdate(const double current_time, const double step_time) -> void override;
7272

73-
void requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) override;
73+
void requestAcquirePosition(
74+
const CanonicalizedLaneletPose & lanelet_pose, const RouteOption &) override;
7475

75-
void requestAcquirePosition(const geometry_msgs::msg::Pose & map_pose) override;
76+
void requestAcquirePosition(
77+
const geometry_msgs::msg::Pose & map_pose, const RouteOption &) override;
7678

7779
void requestWalkStraight() override;
7880

@@ -98,9 +100,11 @@ class PedestrianEntity : public EntityBase
98100

99101
void setDecelerationRateLimit(double deceleration) override;
100102

101-
void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> & waypoints) override;
103+
void requestAssignRoute(
104+
const std::vector<CanonicalizedLaneletPose> & waypoints, const RouteOption &) override;
102105

103-
void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) override;
106+
void requestAssignRoute(
107+
const std::vector<geometry_msgs::msg::Pose> &, const RouteOption &) override;
104108

105109
auto requestFollowTrajectory(
106110
const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void override;

‎simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -98,13 +98,16 @@ class VehicleEntity : public EntityBase
9898

9999
auto onUpdate(const double current_time, const double step_time) -> void override;
100100

101-
void requestAcquirePosition(const CanonicalizedLaneletPose &);
101+
void requestAcquirePosition(const CanonicalizedLaneletPose &, const RouteOption &) override;
102102

103-
void requestAcquirePosition(const geometry_msgs::msg::Pose & map_pose) override;
103+
void requestAcquirePosition(
104+
const geometry_msgs::msg::Pose & map_pose, const RouteOption &) override;
104105

105-
void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) override;
106+
void requestAssignRoute(
107+
const std::vector<geometry_msgs::msg::Pose> &, const RouteOption &) override;
106108

107-
void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> &) override;
109+
void requestAssignRoute(
110+
const std::vector<CanonicalizedLaneletPose> &, const RouteOption &) override;
108111

109112
auto requestFollowTrajectory(
110113
const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void override;

0 commit comments

Comments
 (0)