Merge pull request #1602 from tier4/refactor/behavior-tree-1

Refactor/behavior_tree-1

Author: hakuturu583

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -46,7 +46,6 @@ class ActionNode : public BT::ActionNodeBase
 public:
   ActionNode(const std::string & name, const BT::NodeConfiguration & config);
   ~ActionNode() override = default;
-  auto foundConflictingEntity(const lanelet::Ids & following_lanelets) const -> bool;
   auto getDistanceToConflictingEntity(
     const lanelet::Ids & route_lanelets,
     const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>;
@@ -63,8 +62,6 @@ class ActionNode : public BT::ActionNodeBase
   auto getRightOfWayEntities(const lanelet::Ids & following_lanelets) const
     -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
   auto getYieldStopDistance(const lanelet::Ids & following_lanelets) const -> std::optional<double>;
-  auto getOtherEntityStatus(lanelet::Id lanelet_id) const
-    -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
   auto stopEntity() const -> void;
   auto getHorizon() const -> double;
 
@@ -109,24 +106,24 @@ class ActionNode : public BT::ActionNodeBase
     -> traffic_simulator::EntityStatus;
 
 protected:
-  traffic_simulator::behavior::Request request;
-  std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils;
-  std::shared_ptr<traffic_simulator::TrafficLightsBase> traffic_lights;
-  std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> canonicalized_entity_status;
-  double current_time;
-  double step_time;
-  double default_matching_distance_for_lanelet_pose_calculation;
-  std::optional<double> target_speed;
-  EntityStatusDict other_entity_status;
-  lanelet::Ids route_lanelets;
-  std::shared_ptr<EuclideanDistancesMap> euclidean_distances_map;
-  traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter;
+  traffic_simulator::behavior::Request request_;
+  std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_;
+  std::shared_ptr<traffic_simulator::TrafficLightsBase> traffic_lights_;
+  std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> canonicalized_entity_status_;
+  double current_time_;
+  double step_time_;
+  double default_matching_distance_for_lanelet_pose_calculation_;
+  std::optional<double> target_speed_;
+  EntityStatusDict other_entity_status_;
+  lanelet::Ids route_lanelets_;
+  traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_;
 
   auto getDistanceToTargetEntity(
     const math::geometry::CatmullRomSplineInterface & spline,
     const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional<double>;
 
 private:
+  auto foundConflictingEntity(const lanelet::Ids & following_lanelets) const -> bool;
   auto getDistanceToTargetEntityOnCrosswalk(
     const math::geometry::CatmullRomSplineInterface & spline,
     const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional<double>;
@@ -136,8 +133,12 @@ class ActionNode : public BT::ActionNodeBase
     -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
   auto getConflictingEntityStatusOnLane(const lanelet::Ids & route_lanelets) const
     -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
+  auto getOtherEntityStatus(lanelet::Id lanelet_id) const
+    -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
   auto isOtherEntityAtConsideredAltitude(
     const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool;
+
+  std::shared_ptr<EuclideanDistancesMap> euclidean_distances_map_;
 };
 }  // namespace entity_behavior
 

simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp

@@ -40,7 +40,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
 public:
   void configure(const rclcpp::Logger & logger) override;
   auto update(const double current_time, const double step_time) -> void override;
-  const std::string & getCurrentAction() const override;
+  auto getCurrentAction() -> const std::string & override;
 
 #define DEFINE_GETTER_SETTER(NAME, TYPE)                                                    \
   TYPE get##NAME() override { return tree_.rootBlackboard()->get<TYPE>(get##NAME##Key()); } \

simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp

@@ -40,7 +40,7 @@ class VehicleBehaviorTree : public BehaviorPluginBase
 public:
   auto update(const double current_time, const double step_time) -> void override;
   void configure(const rclcpp::Logger & logger) override;
-  const std::string & getCurrentAction() const override;
+  auto getCurrentAction() -> const std::string & override;
 
   auto getBehaviorParameter() -> traffic_simulator_msgs::msg::BehaviorParameter override;
 

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -78,7 +78,7 @@ auto PedestrianBehaviorTree::createBehaviorTree(const std::string & format_path)
   return factory_.createTreeFromText(xml_str.str());
 }
 
-const std::string & PedestrianBehaviorTree::getCurrentAction() const
+auto PedestrianBehaviorTree::getCurrentAction() -> const std::string &
 {
   return logging_event_ptr_->getCurrentAction();
 }

simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp

@@ -56,9 +56,9 @@ bool FollowLaneAction::detectObstacleInLane(
   auto hasObstacleInPedestrianLanes =
     [this](const lanelet::Ids pedestrian_lanes_local, const double max_detect_length) {
       using math::geometry::operator-;
-      const auto & pedestrian_position = canonicalized_entity_status->getMapPose().position;
+      const auto & pedestrian_position = canonicalized_entity_status_->getMapPose().position;
       lanelet::Ids other_entity_lane_ids;
-      for (const auto & [_, status] : other_entity_status) {
+      for (const auto & [_, status] : other_entity_status_) {
         if (status.getType().type != traffic_simulator_msgs::msg::EntityType::EGO) {
           continue;
         }
@@ -84,9 +84,9 @@ bool FollowLaneAction::detectObstacleInLane(
   auto hasObstacleInFrontOfPedestrian = [this]() {
     using math::geometry::operator-;
 
-    const auto & pedestrian_position = canonicalized_entity_status->getMapPose().position;
+    const auto & pedestrian_position = canonicalized_entity_status_->getMapPose().position;
 
-    for (const auto & [_, entity_status] : other_entity_status) {
+    for (const auto & [_, entity_status] : other_entity_status_) {
       const auto & other_position = entity_status.getMapPose().position;
       const auto relative_position = other_position - pedestrian_position;
       const double relative_angle_rad = std::atan2(relative_position.y, relative_position.x);
@@ -108,25 +108,25 @@ BT::NodeStatus FollowLaneAction::tick()
 {
   getBlackBoardValues();
   if (
-    request != traffic_simulator::behavior::Request::NONE &&
-    request != traffic_simulator::behavior::Request::FOLLOW_LANE) {
+    request_ != traffic_simulator::behavior::Request::NONE &&
+    request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
     return BT::NodeStatus::FAILURE;
   }
-  if (!canonicalized_entity_status->isInLanelet()) {
+  if (!canonicalized_entity_status_->isInLanelet()) {
     stopEntity();
     return BT::NodeStatus::RUNNING;
   }
   auto following_lanelets =
-    hdmap_utils->getFollowingLanelets(canonicalized_entity_status->getLaneletId());
-  if (!target_speed) {
-    target_speed = hdmap_utils->getSpeedLimit(following_lanelets);
+    hdmap_utils_->getFollowingLanelets(canonicalized_entity_status_->getLaneletId());
+  if (!target_speed_) {
+    target_speed_ = hdmap_utils_->getSpeedLimit(following_lanelets);
   }
 
   const auto obstacle_detector_result =
-    detectObstacleInLane(following_lanelets, behavior_parameter.see_around);
-  target_speed = obstacle_detector_result ? 0.0 : target_speed;
+    detectObstacleInLane(following_lanelets, behavior_parameter_.see_around);
+  target_speed_ = obstacle_detector_result ? 0.0 : target_speed_;
 
-  setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value()));
+  setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value()));
   return BT::NodeStatus::RUNNING;
 }
 }  // namespace pedestrian

simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp

@@ -22,7 +22,7 @@ auto FollowPolylineTrajectoryAction::calculateWaypoints()
   -> const traffic_simulator_msgs::msg::WaypointsArray
 {
   auto waypoints = traffic_simulator_msgs::msg::WaypointsArray();
-  waypoints.waypoints.push_back(canonicalized_entity_status->getMapPose().position);
+  waypoints.waypoints.push_back(canonicalized_entity_status_->getMapPose().position);
   for (const auto & vertex : polyline_trajectory->shape.vertices) {
     waypoints.waypoints.push_back(vertex.position.position);
   }
@@ -50,35 +50,35 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList
 {
   auto ports = PedestrianActionNode::providedPorts();
   ports.emplace(BT::InputPort<decltype(polyline_trajectory)>("polyline_trajectory"));
-  ports.emplace(BT::InputPort<decltype(target_speed)>("target_speed"));
+  ports.emplace(BT::InputPort<decltype(target_speed_)>("target_speed"));
   return ports;
 }
 
 auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
 {
   auto getTargetSpeed = [&]() -> double {
-    if (target_speed.has_value()) {
-      return target_speed.value();
+    if (target_speed_.has_value()) {
+      return target_speed_.value();
     } else {
-      return canonicalized_entity_status->getTwist().linear.x;
+      return canonicalized_entity_status_->getTwist().linear.x;
     }
   };
 
   if (getBlackBoardValues();
-      request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
+      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 getInput<decltype(target_speed_)>("target_speed", target_speed_) or
       not polyline_trajectory) {
     return BT::NodeStatus::FAILURE;
-  } else if (std::isnan(canonicalized_entity_status->getTime())) {
+  } 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 (
     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,
-      default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) {
+      static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status_),
+      *polyline_trajectory, behavior_parameter_, hdmap_utils_, step_time_,
+      default_matching_distance_for_lanelet_pose_calculation_, getTargetSpeed())) {
     setCanonicalizedEntityStatus(entity_status_updated.value());
     setOutput("waypoints", calculateWaypoints());
     setOutput("obstacle", calculateObstacle(calculateWaypoints()));

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

@@ -40,20 +40,20 @@ auto PedestrianActionNode::calculateUpdatedEntityStatus(double target_speed) con
   -> traffic_simulator::EntityStatus
 {
   return ActionNode::calculateUpdatedEntityStatus(
-    target_speed, behavior_parameter.dynamic_constraints);
+    target_speed, behavior_parameter_.dynamic_constraints);
 }
 
 auto PedestrianActionNode::calculateUpdatedEntityStatusInWorldFrame(double target_speed) const
   -> traffic_simulator::EntityStatus
 {
   auto entity_status_updated = ActionNode::calculateUpdatedEntityStatusInWorldFrame(
-    target_speed, behavior_parameter.dynamic_constraints);
+    target_speed, behavior_parameter_.dynamic_constraints);
   if (
     const auto canonicalized_lanelet_pose =
       traffic_simulator::pose::pedestrian::transformToCanonicalizedLaneletPose(
-        entity_status_updated.pose, canonicalized_entity_status->getBoundingBox(),
-        canonicalized_entity_status->getLaneletIds(), true,
-        default_matching_distance_for_lanelet_pose_calculation)) {
+        entity_status_updated.pose, canonicalized_entity_status_->getBoundingBox(),
+        canonicalized_entity_status_->getLaneletIds(), true,
+        default_matching_distance_for_lanelet_pose_calculation_)) {
     entity_status_updated.lanelet_pose_valid = true;
     entity_status_updated.lanelet_pose =
       static_cast<traffic_simulator::LaneletPose>(canonicalized_lanelet_pose.value());

simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp

@@ -41,13 +41,13 @@ void WalkStraightAction::getBlackBoardValues() { PedestrianActionNode::getBlackB
 BT::NodeStatus WalkStraightAction::tick()
 {
   getBlackBoardValues();
-  if (request != traffic_simulator::behavior::Request::WALK_STRAIGHT) {
+  if (request_ != traffic_simulator::behavior::Request::WALK_STRAIGHT) {
     return BT::NodeStatus::FAILURE;
   }
-  if (!target_speed) {
-    target_speed = 1.111;
+  if (!target_speed_) {
+    target_speed_ = 1.111;
   }
-  setCanonicalizedEntityStatus(calculateUpdatedEntityStatusInWorldFrame(target_speed.value()));
+  setCanonicalizedEntityStatus(calculateUpdatedEntityStatusInWorldFrame(target_speed_.value()));
   return BT::NodeStatus::RUNNING;
 }
 }  // namespace pedestrian

simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp

@@ -137,7 +137,7 @@ auto VehicleBehaviorTree::setBehaviorParameter(
     getBehaviorParameterKey(), clamp(behavior_parameter));
 }
 
-const std::string & VehicleBehaviorTree::getCurrentAction() const
+auto VehicleBehaviorTree::getCurrentAction() -> const std::string &
 {
   return logging_event_ptr_->getCurrentAction();
 }

simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp

@@ -52,13 +52,13 @@ FollowFrontEntityAction::calculateObstacle(const traffic_simulator_msgs::msg::Wa
 
 const traffic_simulator_msgs::msg::WaypointsArray FollowFrontEntityAction::calculateWaypoints()
 {
-  if (!canonicalized_entity_status->isInLanelet()) {
+  if (!canonicalized_entity_status_->isInLanelet()) {
     THROW_SIMULATION_ERROR("failed to assign lane");
   }
-  if (canonicalized_entity_status->getTwist().linear.x >= 0) {
+  if (canonicalized_entity_status_->getTwist().linear.x >= 0) {
     traffic_simulator_msgs::msg::WaypointsArray waypoints;
     double horizon = getHorizon();
-    const auto lanelet_pose = canonicalized_entity_status->getLaneletPose();
+    const auto lanelet_pose = canonicalized_entity_status_->getLaneletPose();
     waypoints.waypoints = reference_trajectory->getTrajectory(
       lanelet_pose.s, lanelet_pose.s + horizon, 1.0, lanelet_pose.offset);
     trajectory = std::make_unique<math::geometry::CatmullRomSubspline>(
@@ -73,14 +73,14 @@ BT::NodeStatus FollowFrontEntityAction::tick()
 {
   getBlackBoardValues();
   if (
-    request != traffic_simulator::behavior::Request::NONE &&
-    request != traffic_simulator::behavior::Request::FOLLOW_LANE) {
+    request_ != traffic_simulator::behavior::Request::NONE &&
+    request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
     return BT::NodeStatus::FAILURE;
   }
-  if (getRightOfWayEntities(route_lanelets).size() != 0) {
+  if (getRightOfWayEntities(route_lanelets_).size() != 0) {
     return BT::NodeStatus::FAILURE;
   }
-  if (!behavior_parameter.see_around) {
+  if (!behavior_parameter_.see_around) {
     return BT::NodeStatus::FAILURE;
   }
   const auto waypoints = calculateWaypoints();
@@ -91,8 +91,9 @@ BT::NodeStatus FollowFrontEntityAction::tick()
     return BT::NodeStatus::FAILURE;
   }
   auto distance_to_stopline =
-    traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory);
-  auto distance_to_conflicting_entity = getDistanceToConflictingEntity(route_lanelets, *trajectory);
+    traffic_simulator::distance::distanceToStopLine(route_lanelets_, *trajectory);
+  auto distance_to_conflicting_entity =
+    getDistanceToConflictingEntity(route_lanelets_, *trajectory);
   const auto front_entity_name = getFrontEntityName(*trajectory);
   if (!front_entity_name) {
     return BT::NodeStatus::FAILURE;
@@ -112,28 +113,28 @@ BT::NodeStatus FollowFrontEntityAction::tick()
       return BT::NodeStatus::FAILURE;
     }
   }
-  if (!target_speed) {
-    target_speed = hdmap_utils->getSpeedLimit(route_lanelets);
+  if (!target_speed_) {
+    target_speed_ = hdmap_utils_->getSpeedLimit(route_lanelets_);
   }
   const double front_entity_linear_velocity = front_entity_status.getTwist().linear.x;
-  if (target_speed.value() <= front_entity_linear_velocity) {
-    setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value()));
+  if (target_speed_.value() <= front_entity_linear_velocity) {
+    setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value()));
     const auto obstacle = calculateObstacle(waypoints);
     setOutput("waypoints", waypoints);
     setOutput("obstacle", obstacle);
     return BT::NodeStatus::RUNNING;
   }
   if (
     distance_to_front_entity_.value() >=
-    (calculateStopDistance(behavior_parameter.dynamic_constraints) +
+    (calculateStopDistance(behavior_parameter_.dynamic_constraints) +
      vehicle_parameters.bounding_box.dimensions.x + 5)) {
     setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity + 2));
     setOutput("waypoints", waypoints);
     setOutput("obstacle", calculateObstacle(waypoints));
     return BT::NodeStatus::RUNNING;
   } else if (
     distance_to_front_entity_.value() <=
-    calculateStopDistance(behavior_parameter.dynamic_constraints)) {
+    calculateStopDistance(behavior_parameter_.dynamic_constraints)) {
     setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity - 2));
     setOutput("waypoints", waypoints);
     setOutput("obstacle", calculateObstacle(waypoints));