Skip to content

Commit 3b708d2

Browse files
hakuturu583hakuturu583
authored
Merge pull request #1612 from tier4/refactor/behavior_tree_5
Refactor ActionNode to isolate side effects via pure virtual methods
2 parents e9151b2 + 72ff550 commit 3b708d2

26 files changed

+181
-113
lines changed

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -118,6 +118,8 @@ class ActionNode : public BT::ActionNodeBase
118118
lanelet::Ids route_lanelets_;
119119
traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_;
120120

121+
virtual bool checkPreconditions() { return true; }
122+
virtual BT::NodeStatus doAction() = 0;
121123
auto getDistanceToTargetEntity(
122124
const math::geometry::CatmullRomSplineInterface & spline,
123125
const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional<double>;
@@ -137,6 +139,7 @@ class ActionNode : public BT::ActionNodeBase
137139
-> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
138140
auto isOtherEntityAtConsideredAltitude(
139141
const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool;
142+
auto tick() -> BT::NodeStatus override;
140143

141144
std::shared_ptr<EuclideanDistancesMap> euclidean_distances_map_;
142145
};

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_lane_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,8 @@ class FollowLaneAction : public entity_behavior::PedestrianActionNode
3939
{
4040
public:
4141
FollowLaneAction(const std::string & name, const BT::NodeConfiguration & config);
42-
BT::NodeStatus tick() override;
42+
bool checkPreconditions() override;
43+
BT::NodeStatus doAction() override;
4344
void getBlackBoardValues() override;
4445
static BT::PortsList providedPorts()
4546
{

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,8 @@ struct FollowPolylineTrajectoryAction : public PedestrianActionNode
3434

3535
static auto providedPorts() -> BT::PortsList;
3636

37-
auto tick() -> BT::NodeStatus override;
37+
bool checkPreconditions() override;
38+
BT::NodeStatus doAction() override;
3839
};
3940
} // namespace pedestrian
4041
} // namespace entity_behavior

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/walk_straight_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,8 @@ class WalkStraightAction : public entity_behavior::PedestrianActionNode
4444
{
4545
public:
4646
WalkStraightAction(const std::string & name, const BT::NodeConfiguration & config);
47-
BT::NodeStatus tick() override;
47+
bool checkPreconditions() override;
48+
BT::NodeStatus doAction() override;
4849
void getBlackBoardValues() override;
4950
static BT::PortsList providedPorts()
5051
{

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/follow_front_entity_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,8 @@ class FollowFrontEntityAction : public entity_behavior::VehicleActionNode
3131
{
3232
public:
3333
FollowFrontEntityAction(const std::string & name, const BT::NodeConfiguration & config);
34-
BT::NodeStatus tick() override;
34+
bool checkPreconditions() override;
35+
BT::NodeStatus doAction() override;
3536
static BT::PortsList providedPorts()
3637
{
3738
return entity_behavior::VehicleActionNode::providedPorts();

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/follow_lane_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,8 @@ class FollowLaneAction : public entity_behavior::VehicleActionNode
3131
{
3232
public:
3333
FollowLaneAction(const std::string & name, const BT::NodeConfiguration & config);
34-
BT::NodeStatus tick() override;
34+
bool checkPreconditions() override;
35+
BT::NodeStatus doAction() override;
3536
void getBlackBoardValues() override;
3637
static BT::PortsList providedPorts()
3738
{

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/move_backward_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,8 @@ class MoveBackwardAction : public entity_behavior::VehicleActionNode
3131
{
3232
public:
3333
MoveBackwardAction(const std::string & name, const BT::NodeConfiguration & config);
34-
BT::NodeStatus tick() override;
34+
bool checkPreconditions() override;
35+
BT::NodeStatus doAction() override;
3536
void getBlackBoardValues() override;
3637
static BT::PortsList providedPorts()
3738
{

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,8 @@ class StopAtCrossingEntityAction : public entity_behavior::VehicleActionNode
3131
{
3232
public:
3333
StopAtCrossingEntityAction(const std::string & name, const BT::NodeConfiguration & config);
34-
BT::NodeStatus tick() override;
34+
bool checkPreconditions() override;
35+
BT::NodeStatus doAction() override;
3536
static BT::PortsList providedPorts()
3637
{
3738
return entity_behavior::VehicleActionNode::providedPorts();

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_stop_line_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,8 @@ class StopAtStopLineAction : public entity_behavior::VehicleActionNode
3131
{
3232
public:
3333
StopAtStopLineAction(const std::string & name, const BT::NodeConfiguration & config);
34-
BT::NodeStatus tick() override;
34+
bool checkPreconditions() override;
35+
BT::NodeStatus doAction() override;
3536
static BT::PortsList providedPorts()
3637
{
3738
return entity_behavior::VehicleActionNode::providedPorts();

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_traffic_light_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,8 @@ class StopAtTrafficLightAction : public entity_behavior::VehicleActionNode
3131
{
3232
public:
3333
StopAtTrafficLightAction(const std::string & name, const BT::NodeConfiguration & config);
34-
BT::NodeStatus tick() override;
34+
bool checkPreconditions() override;
35+
BT::NodeStatus doAction() override;
3536
static BT::PortsList providedPorts()
3637
{
3738
return entity_behavior::VehicleActionNode::providedPorts();

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/yield_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,8 @@ class YieldAction : public entity_behavior::VehicleActionNode
3131
{
3232
public:
3333
YieldAction(const std::string & name, const BT::NodeConfiguration & config);
34-
BT::NodeStatus tick() override;
34+
bool checkPreconditions() override;
35+
BT::NodeStatus doAction() override;
3536
static BT::PortsList providedPorts()
3637
{
3738
return entity_behavior::VehicleActionNode::providedPorts();

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,8 @@ struct FollowPolylineTrajectoryAction : public VehicleActionNode
3434

3535
static auto providedPorts() -> BT::PortsList;
3636

37-
auto tick() -> BT::NodeStatus override;
37+
bool checkPreconditions() override;
38+
BT::NodeStatus doAction() override;
3839
};
3940
} // namespace vehicle
4041
} // namespace entity_behavior

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/lane_change_action.hpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,8 @@ class LaneChangeAction : public entity_behavior::VehicleActionNode
3434
{
3535
public:
3636
LaneChangeAction(const std::string & name, const BT::NodeConfiguration & config);
37-
BT::NodeStatus tick() override;
37+
bool checkPreconditions() override;
38+
BT::NodeStatus doAction() override;
3839
static BT::PortsList providedPorts()
3940
{
4041
return BT::PortsList(

‎simulation/behavior_tree_plugin/src/action_node.cpp

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,15 @@ ActionNode::ActionNode(const std::string & name, const BT::NodeConfiguration & c
5151

5252
auto ActionNode::executeTick() -> BT::NodeStatus { return BT::ActionNodeBase::executeTick(); }
5353

54+
auto ActionNode::tick() -> BT::NodeStatus
55+
{
56+
getBlackBoardValues();
57+
if (!checkPreconditions()) {
58+
return BT::NodeStatus::FAILURE;
59+
}
60+
return doAction();
61+
}
62+
5463
auto ActionNode::getBlackBoardValues() -> void
5564
{
5665
if (!getInput<traffic_simulator::behavior::Request>("request", request_)) {

‎simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -103,15 +103,19 @@ bool FollowLaneAction::detectObstacleInLane(
103103
return false;
104104
}
105105
}
106-
107-
BT::NodeStatus FollowLaneAction::tick()
106+
bool FollowLaneAction::checkPreconditions()
108107
{
109-
getBlackBoardValues();
110108
if (
111109
request_ != traffic_simulator::behavior::Request::NONE &&
112110
request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
113-
return BT::NodeStatus::FAILURE;
111+
return false;
112+
} else {
113+
return true;
114114
}
115+
}
116+
117+
BT::NodeStatus FollowLaneAction::doAction()
118+
{
115119
if (!canonicalized_entity_status_->isInLanelet()) {
116120
stopEntity();
117121
return BT::NodeStatus::RUNNING;

‎simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

Lines changed: 18 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -54,27 +54,34 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList
5454
return ports;
5555
}
5656

57-
auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
57+
bool FollowPolylineTrajectoryAction::checkPreconditions()
5858
{
59-
auto getTargetSpeed = [&]() -> double {
60-
if (target_speed_.has_value()) {
61-
return target_speed_.value();
62-
} else {
63-
return canonicalized_entity_status_->getTwist().linear.x;
64-
}
65-
};
66-
6759
if (getBlackBoardValues();
6860
request_ != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
6961
not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
7062
not getInput<decltype(target_speed_)>("target_speed", target_speed_) or
7163
not polyline_trajectory) {
72-
return BT::NodeStatus::FAILURE;
64+
return false;
7365
} else if (std::isnan(canonicalized_entity_status_->getTime())) {
7466
THROW_SIMULATION_ERROR(
7567
"Time in canonicalized_entity_status is NaN - FollowTrajectoryAction does not support such "
7668
"case.");
77-
} else if (
69+
} else {
70+
return true;
71+
}
72+
}
73+
74+
auto FollowPolylineTrajectoryAction::doAction() -> BT::NodeStatus
75+
{
76+
auto getTargetSpeed = [&]() -> double {
77+
if (target_speed_.has_value()) {
78+
return target_speed_.value();
79+
} else {
80+
return canonicalized_entity_status_->getTwist().linear.x;
81+
}
82+
};
83+
84+
if (
7885
const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus(
7986
static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status_),
8087
*polyline_trajectory, behavior_parameter_, hdmap_utils_, step_time_,

‎simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -38,12 +38,13 @@ WalkStraightAction::WalkStraightAction(
3838

3939
void WalkStraightAction::getBlackBoardValues() { PedestrianActionNode::getBlackBoardValues(); }
4040

41-
BT::NodeStatus WalkStraightAction::tick()
41+
bool WalkStraightAction::checkPreconditions()
42+
{
43+
return request_ == traffic_simulator::behavior::Request::WALK_STRAIGHT;
44+
}
45+
46+
BT::NodeStatus WalkStraightAction::doAction()
4247
{
43-
getBlackBoardValues();
44-
if (request_ != traffic_simulator::behavior::Request::WALK_STRAIGHT) {
45-
return BT::NodeStatus::FAILURE;
46-
}
4748
if (!target_speed_) {
4849
target_speed_ = 1.111;
4950
}

‎simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp

Lines changed: 12 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -69,20 +69,23 @@ const traffic_simulator_msgs::msg::WaypointsArray FollowFrontEntityAction::calcu
6969
}
7070
}
7171

72-
BT::NodeStatus FollowFrontEntityAction::tick()
72+
bool FollowFrontEntityAction::checkPreconditions()
7373
{
74-
getBlackBoardValues();
7574
if (
7675
request_ != traffic_simulator::behavior::Request::NONE &&
7776
request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
78-
return BT::NodeStatus::FAILURE;
79-
}
80-
if (getRightOfWayEntities(route_lanelets_).size() != 0) {
81-
return BT::NodeStatus::FAILURE;
82-
}
83-
if (!behavior_parameter_.see_around) {
84-
return BT::NodeStatus::FAILURE;
77+
return false;
78+
} else if (!getRightOfWayEntities(route_lanelets_).empty()) {
79+
return false;
80+
} else if (!behavior_parameter_.see_around) {
81+
return false;
82+
} else {
83+
return true;
8584
}
85+
}
86+
87+
BT::NodeStatus FollowFrontEntityAction::doAction()
88+
{
8689
const auto waypoints = calculateWaypoints();
8790
if (waypoints.waypoints.empty()) {
8891
return BT::NodeStatus::FAILURE;

‎simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -66,14 +66,19 @@ void FollowLaneAction::getBlackBoardValues()
6666
}
6767
}
6868

69-
BT::NodeStatus FollowLaneAction::tick()
69+
bool FollowLaneAction::checkPreconditions()
7070
{
71-
getBlackBoardValues();
7271
if (
7372
request_ != traffic_simulator::behavior::Request::NONE &&
7473
request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
75-
return BT::NodeStatus::FAILURE;
74+
return false;
75+
} else {
76+
return true;
7677
}
78+
}
79+
80+
BT::NodeStatus FollowLaneAction::doAction()
81+
{
7782
if (!canonicalized_entity_status_->isInLanelet()) {
7883
stopEntity();
7984
const auto waypoints = traffic_simulator_msgs::msg::WaypointsArray{};

‎simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -62,17 +62,21 @@ const traffic_simulator_msgs::msg::WaypointsArray MoveBackwardAction::calculateW
6262

6363
void MoveBackwardAction::getBlackBoardValues() { VehicleActionNode::getBlackBoardValues(); }
6464

65-
BT::NodeStatus MoveBackwardAction::tick()
65+
bool MoveBackwardAction::checkPreconditions()
6666
{
67-
getBlackBoardValues();
6867
if (
6968
request_ != traffic_simulator::behavior::Request::NONE &&
7069
request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
71-
return BT::NodeStatus::FAILURE;
72-
}
73-
if (!canonicalized_entity_status_->isInLanelet()) {
74-
return BT::NodeStatus::FAILURE;
70+
return false;
71+
} else if (!canonicalized_entity_status_->isInLanelet()) {
72+
return false;
73+
} else {
74+
return true;
7575
}
76+
}
77+
78+
BT::NodeStatus MoveBackwardAction::doAction()
79+
{
7680
const auto waypoints = calculateWaypoints();
7781
if (waypoints.waypoints.empty()) {
7882
return BT::NodeStatus::FAILURE;

‎simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp

Lines changed: 14 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -82,23 +82,25 @@ std::optional<double> StopAtCrossingEntityAction::calculateTargetSpeed(double cu
8282
return current_velocity;
8383
}
8484

85-
BT::NodeStatus StopAtCrossingEntityAction::tick()
85+
bool StopAtCrossingEntityAction::checkPreconditions()
8686
{
87-
getBlackBoardValues();
8887
if (
8988
request_ != traffic_simulator::behavior::Request::NONE &&
9089
request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
91-
return BT::NodeStatus::FAILURE;
92-
}
93-
if (!canonicalized_entity_status_->isInLanelet()) {
94-
return BT::NodeStatus::FAILURE;
95-
}
96-
if (!behavior_parameter_.see_around) {
97-
return BT::NodeStatus::FAILURE;
98-
}
99-
if (getRightOfWayEntities(route_lanelets_).size() != 0) {
100-
return BT::NodeStatus::FAILURE;
90+
return false;
91+
} else if (!canonicalized_entity_status_->isInLanelet()) {
92+
return false;
93+
} else if (!behavior_parameter_.see_around) {
94+
return false;
95+
} else if (!getRightOfWayEntities(route_lanelets_).empty()) {
96+
return false;
97+
} else {
98+
return true;
10199
}
100+
}
101+
102+
BT::NodeStatus StopAtCrossingEntityAction::doAction()
103+
{
102104
const auto waypoints = calculateWaypoints();
103105
if (waypoints.waypoints.empty()) {
104106
return BT::NodeStatus::FAILURE;

0 commit comments

Comments
 (0)