Refactor ActionNode to isolate side effects via pure virtual methods

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -118,6 +118,8 @@ class ActionNode : public BT::ActionNodeBase
   lanelet::Ids route_lanelets_;
   traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_;
 
+  virtual bool checkPreconditions() { return true; }
+  virtual BT::NodeStatus doAction() = 0;
   auto getDistanceToTargetEntity(
     const math::geometry::CatmullRomSplineInterface & spline,
     const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional<double>;
@@ -137,6 +139,7 @@ class ActionNode : public BT::ActionNodeBase
     -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
   auto isOtherEntityAtConsideredAltitude(
     const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool;
+  auto tick() -> BT::NodeStatus override;
 
   std::shared_ptr<EuclideanDistancesMap> euclidean_distances_map_;
 };

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

@@ -39,7 +39,8 @@ class FollowLaneAction : public entity_behavior::PedestrianActionNode
 {
 public:
   FollowLaneAction(const std::string & name, const BT::NodeConfiguration & config);
-  BT::NodeStatus tick() override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
   void getBlackBoardValues() override;
   static BT::PortsList providedPorts()
   {

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

@@ -34,7 +34,8 @@ struct FollowPolylineTrajectoryAction : public PedestrianActionNode
 
   static auto providedPorts() -> BT::PortsList;
 
-  auto tick() -> BT::NodeStatus override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
 };
 }  // namespace pedestrian
 }  // namespace entity_behavior

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

@@ -44,7 +44,8 @@ class WalkStraightAction : public entity_behavior::PedestrianActionNode
 {
 public:
   WalkStraightAction(const std::string & name, const BT::NodeConfiguration & config);
-  BT::NodeStatus tick() override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
   void getBlackBoardValues() override;
   static BT::PortsList providedPorts()
   {

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

@@ -31,7 +31,8 @@ class FollowFrontEntityAction : public entity_behavior::VehicleActionNode
 {
 public:
   FollowFrontEntityAction(const std::string & name, const BT::NodeConfiguration & config);
-  BT::NodeStatus tick() override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
   static BT::PortsList providedPorts()
   {
     return entity_behavior::VehicleActionNode::providedPorts();

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

@@ -31,7 +31,8 @@ class FollowLaneAction : public entity_behavior::VehicleActionNode
 {
 public:
   FollowLaneAction(const std::string & name, const BT::NodeConfiguration & config);
-  BT::NodeStatus tick() override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
   void getBlackBoardValues() override;
   static BT::PortsList providedPorts()
   {

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

@@ -31,7 +31,8 @@ class MoveBackwardAction : public entity_behavior::VehicleActionNode
 {
 public:
   MoveBackwardAction(const std::string & name, const BT::NodeConfiguration & config);
-  BT::NodeStatus tick() override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
   void getBlackBoardValues() override;
   static BT::PortsList providedPorts()
   {

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

@@ -31,7 +31,8 @@ class StopAtCrossingEntityAction : public entity_behavior::VehicleActionNode
 {
 public:
   StopAtCrossingEntityAction(const std::string & name, const BT::NodeConfiguration & config);
-  BT::NodeStatus tick() override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
   static BT::PortsList providedPorts()
   {
     return entity_behavior::VehicleActionNode::providedPorts();

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

@@ -31,7 +31,8 @@ class StopAtStopLineAction : public entity_behavior::VehicleActionNode
 {
 public:
   StopAtStopLineAction(const std::string & name, const BT::NodeConfiguration & config);
-  BT::NodeStatus tick() override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
   static BT::PortsList providedPorts()
   {
     return entity_behavior::VehicleActionNode::providedPorts();

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

@@ -31,7 +31,8 @@ class StopAtTrafficLightAction : public entity_behavior::VehicleActionNode
 {
 public:
   StopAtTrafficLightAction(const std::string & name, const BT::NodeConfiguration & config);
-  BT::NodeStatus tick() override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
   static BT::PortsList providedPorts()
   {
     return entity_behavior::VehicleActionNode::providedPorts();

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

@@ -31,7 +31,8 @@ class YieldAction : public entity_behavior::VehicleActionNode
 {
 public:
   YieldAction(const std::string & name, const BT::NodeConfiguration & config);
-  BT::NodeStatus tick() override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
   static BT::PortsList providedPorts()
   {
     return entity_behavior::VehicleActionNode::providedPorts();

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

@@ -34,7 +34,8 @@ struct FollowPolylineTrajectoryAction : public VehicleActionNode
 
   static auto providedPorts() -> BT::PortsList;
 
-  auto tick() -> BT::NodeStatus override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
 };
 }  // namespace vehicle
 }  // namespace entity_behavior

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

@@ -34,7 +34,8 @@ class LaneChangeAction : public entity_behavior::VehicleActionNode
 {
 public:
   LaneChangeAction(const std::string & name, const BT::NodeConfiguration & config);
-  BT::NodeStatus tick() override;
+  bool checkPreconditions() override;
+  BT::NodeStatus doAction() override;
   static BT::PortsList providedPorts()
   {
     return BT::PortsList(

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -51,6 +51,15 @@ ActionNode::ActionNode(const std::string & name, const BT::NodeConfiguration & c
 
 auto ActionNode::executeTick() -> BT::NodeStatus { return BT::ActionNodeBase::executeTick(); }
 
+auto ActionNode::tick() -> BT::NodeStatus
+{
+  getBlackBoardValues();
+  if (!checkPreconditions()) {
+    return BT::NodeStatus::FAILURE;
+  }
+  return doAction();
+}
+
 auto ActionNode::getBlackBoardValues() -> void
 {
   if (!getInput<traffic_simulator::behavior::Request>("request", request_)) {

simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp

@@ -103,15 +103,19 @@ bool FollowLaneAction::detectObstacleInLane(
     return false;
   }
 }
-
-BT::NodeStatus FollowLaneAction::tick()
+bool FollowLaneAction::checkPreconditions()
 {
-  getBlackBoardValues();
   if (
     request_ != traffic_simulator::behavior::Request::NONE &&
     request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
-    return BT::NodeStatus::FAILURE;
+    return false;
+  } else {
+    return true;
   }
+}
+
+BT::NodeStatus FollowLaneAction::doAction()
+{
   if (!canonicalized_entity_status_->isInLanelet()) {
     stopEntity();
     return BT::NodeStatus::RUNNING;

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

@@ -54,27 +54,34 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList
   return ports;
 }
 
-auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
+bool FollowPolylineTrajectoryAction::checkPreconditions()
 {
-  auto getTargetSpeed = [&]() -> double {
-    if (target_speed_.has_value()) {
-      return target_speed_.value();
-    } else {
-      return canonicalized_entity_status_->getTwist().linear.x;
-    }
-  };
-
   if (getBlackBoardValues();
       request_ != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
       not getInput<decltype(target_speed_)>("target_speed", target_speed_) or
       not polyline_trajectory) {
-    return BT::NodeStatus::FAILURE;
+    return false;
   } else if (std::isnan(canonicalized_entity_status_->getTime())) {
     THROW_SIMULATION_ERROR(
       "Time in canonicalized_entity_status is NaN - FollowTrajectoryAction does not support such "
       "case.");
-  } else if (
+  } else {
+    return true;
+  }
+}
+
+auto FollowPolylineTrajectoryAction::doAction() -> BT::NodeStatus
+{
+  auto getTargetSpeed = [&]() -> double {
+    if (target_speed_.has_value()) {
+      return target_speed_.value();
+    } else {
+      return canonicalized_entity_status_->getTwist().linear.x;
+    }
+  };
+
+  if (
     const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus(
       static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status_),
       *polyline_trajectory, behavior_parameter_, hdmap_utils_, step_time_,

simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp

@@ -38,12 +38,13 @@ WalkStraightAction::WalkStraightAction(
 
 void WalkStraightAction::getBlackBoardValues() { PedestrianActionNode::getBlackBoardValues(); }
 
-BT::NodeStatus WalkStraightAction::tick()
+bool WalkStraightAction::checkPreconditions()
+{
+  return request_ == traffic_simulator::behavior::Request::WALK_STRAIGHT;
+}
+
+BT::NodeStatus WalkStraightAction::doAction()
 {
-  getBlackBoardValues();
-  if (request_ != traffic_simulator::behavior::Request::WALK_STRAIGHT) {
-    return BT::NodeStatus::FAILURE;
-  }
   if (!target_speed_) {
     target_speed_ = 1.111;
   }

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

@@ -69,20 +69,23 @@ const traffic_simulator_msgs::msg::WaypointsArray FollowFrontEntityAction::calcu
   }
 }
 
-BT::NodeStatus FollowFrontEntityAction::tick()
+bool FollowFrontEntityAction::checkPreconditions()
 {
-  getBlackBoardValues();
   if (
     request_ != traffic_simulator::behavior::Request::NONE &&
     request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
-    return BT::NodeStatus::FAILURE;
-  }
-  if (getRightOfWayEntities(route_lanelets_).size() != 0) {
-    return BT::NodeStatus::FAILURE;
-  }
-  if (!behavior_parameter_.see_around) {
-    return BT::NodeStatus::FAILURE;
+    return false;
+  } else if (!getRightOfWayEntities(route_lanelets_).empty()) {
+    return false;
+  } else if (!behavior_parameter_.see_around) {
+    return false;
+  } else {
+    return true;
   }
+}
+
+BT::NodeStatus FollowFrontEntityAction::doAction()
+{
   const auto waypoints = calculateWaypoints();
   if (waypoints.waypoints.empty()) {
     return BT::NodeStatus::FAILURE;

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

@@ -66,14 +66,19 @@ void FollowLaneAction::getBlackBoardValues()
   }
 }
 
-BT::NodeStatus FollowLaneAction::tick()
+bool FollowLaneAction::checkPreconditions()
 {
-  getBlackBoardValues();
   if (
     request_ != traffic_simulator::behavior::Request::NONE &&
     request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
-    return BT::NodeStatus::FAILURE;
+    return false;
+  } else {
+    return true;
   }
+}
+
+BT::NodeStatus FollowLaneAction::doAction()
+{
   if (!canonicalized_entity_status_->isInLanelet()) {
     stopEntity();
     const auto waypoints = traffic_simulator_msgs::msg::WaypointsArray{};

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

@@ -62,17 +62,21 @@ const traffic_simulator_msgs::msg::WaypointsArray MoveBackwardAction::calculateW
 
 void MoveBackwardAction::getBlackBoardValues() { VehicleActionNode::getBlackBoardValues(); }
 
-BT::NodeStatus MoveBackwardAction::tick()
+bool MoveBackwardAction::checkPreconditions()
 {
-  getBlackBoardValues();
   if (
     request_ != traffic_simulator::behavior::Request::NONE &&
     request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
-    return BT::NodeStatus::FAILURE;
-  }
-  if (!canonicalized_entity_status_->isInLanelet()) {
-    return BT::NodeStatus::FAILURE;
+    return false;
+  } else if (!canonicalized_entity_status_->isInLanelet()) {
+    return false;
+  } else {
+    return true;
   }
+}
+
+BT::NodeStatus MoveBackwardAction::doAction()
+{
   const auto waypoints = calculateWaypoints();
   if (waypoints.waypoints.empty()) {
     return BT::NodeStatus::FAILURE;

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

@@ -82,23 +82,25 @@ std::optional<double> StopAtCrossingEntityAction::calculateTargetSpeed(double cu
   return current_velocity;
 }
 
-BT::NodeStatus StopAtCrossingEntityAction::tick()
+bool StopAtCrossingEntityAction::checkPreconditions()
 {
-  getBlackBoardValues();
   if (
     request_ != traffic_simulator::behavior::Request::NONE &&
     request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
-    return BT::NodeStatus::FAILURE;
-  }
-  if (!canonicalized_entity_status_->isInLanelet()) {
-    return BT::NodeStatus::FAILURE;
-  }
-  if (!behavior_parameter_.see_around) {
-    return BT::NodeStatus::FAILURE;
-  }
-  if (getRightOfWayEntities(route_lanelets_).size() != 0) {
-    return BT::NodeStatus::FAILURE;
+    return false;
+  } else if (!canonicalized_entity_status_->isInLanelet()) {
+    return false;
+  } else if (!behavior_parameter_.see_around) {
+    return false;
+  } else if (!getRightOfWayEntities(route_lanelets_).empty()) {
+    return false;
+  } else {
+    return true;
   }
+}
+
+BT::NodeStatus StopAtCrossingEntityAction::doAction()
+{
   const auto waypoints = calculateWaypoints();
   if (waypoints.waypoints.empty()) {
     return BT::NodeStatus::FAILURE;