Refactor/behavior tree 3

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

@@ -42,6 +42,9 @@ class FollowFrontEntityAction : public entity_behavior::VehicleActionNode
 
 private:
   std::optional<double> distance_to_front_entity_;
+  static constexpr double kWaypointInterval = 1.0;
+  static constexpr double kFrontEntityMargin = 5.0;
+  static constexpr double kSpeedStep = 2.0;
 };
 }  // namespace follow_lane_sequence
 }  // namespace vehicle

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

@@ -43,6 +43,11 @@ class FollowLaneAction : public entity_behavior::VehicleActionNode
 
 private:
   std::optional<traffic_simulator::LaneletPose> target_lanelet_pose_;
+  static constexpr double kWaypointInterval = 1.0;
+  static constexpr double kFrontEntityStopMargin = 5.0;
+  static constexpr double kBoundingBoxHalfFactor = 0.5;
+  static constexpr double kStopLineMargin = 5.0;
+  static constexpr double kConflictingEntityMargin = 3.0;
 };
 }  // namespace follow_lane_sequence
 }  // namespace vehicle

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

@@ -43,7 +43,6 @@ class StopAtCrossingEntityAction : public entity_behavior::VehicleActionNode
 
 private:
   std::optional<double> distance_to_stop_target_;
-  bool in_stop_sequence_;
 };
 }  // namespace follow_lane_sequence
 }  // namespace vehicle

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

@@ -42,8 +42,12 @@ class StopAtStopLineAction : public entity_behavior::VehicleActionNode
     const traffic_simulator_msgs::msg::WaypointsArray & waypoints) override;
 
 private:
-  bool stopped_;
   std::optional<double> distance_to_stopline_;
+  static constexpr double kWaypointInterval = 1.0;
+  static constexpr double kBoundingBoxHalfFactor = 0.5;
+  static constexpr double kStopMargin = 1.0;
+  static constexpr double kFrontStoplineMargin = 5.0;
+  static constexpr double kVelocityEpsilon = 0.001;
 };
 }  // namespace follow_lane_sequence
 }  // namespace vehicle

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

@@ -60,7 +60,7 @@ const traffic_simulator_msgs::msg::WaypointsArray FollowFrontEntityAction::calcu
     double horizon = getHorizon();
     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);
+      lanelet_pose.s, lanelet_pose.s + horizon, kWaypointInterval, lanelet_pose.offset);
     trajectory = std::make_unique<math::geometry::CatmullRomSubspline>(
       reference_trajectory, lanelet_pose.s, lanelet_pose.s + horizon);
     return waypoints;
@@ -127,15 +127,17 @@ BT::NodeStatus FollowFrontEntityAction::tick()
   if (
     distance_to_front_entity_.value() >=
     (calculateStopDistance(behavior_parameter_.dynamic_constraints) +
-     vehicle_parameters.bounding_box.dimensions.x + 5)) {
-    setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity + 2));
+     vehicle_parameters.bounding_box.dimensions.x + kFrontEntityMargin)) {
+    setCanonicalizedEntityStatus(
+      calculateUpdatedEntityStatus(front_entity_linear_velocity + kSpeedStep));
     setOutput("waypoints", waypoints);
     setOutput("obstacle", calculateObstacle(waypoints));
     return BT::NodeStatus::RUNNING;
   } else if (
     distance_to_front_entity_.value() <=
     calculateStopDistance(behavior_parameter_.dynamic_constraints)) {
-    setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity - 2));
+    setCanonicalizedEntityStatus(
+      calculateUpdatedEntityStatus(front_entity_linear_velocity - kSpeedStep));
     setOutput("waypoints", waypoints);
     setOutput("obstacle", calculateObstacle(waypoints));
     return BT::NodeStatus::RUNNING;

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

@@ -46,7 +46,7 @@ const traffic_simulator_msgs::msg::WaypointsArray FollowLaneAction::calculateWay
     traffic_simulator_msgs::msg::WaypointsArray waypoints;
     const auto lanelet_pose = canonicalized_entity_status_->getLaneletPose();
     waypoints.waypoints = reference_trajectory->getTrajectory(
-      lanelet_pose.s, lanelet_pose.s + getHorizon(), 1.0, lanelet_pose.offset);
+      lanelet_pose.s, lanelet_pose.s + getHorizon(), kWaypointInterval, lanelet_pose.offset);
     trajectory = std::make_unique<math::geometry::CatmullRomSubspline>(
       reference_trajectory, lanelet_pose.s, lanelet_pose.s + getHorizon());
     return waypoints;
@@ -97,7 +97,7 @@ BT::NodeStatus FollowLaneAction::tick()
       if (
         distance_to_front_entity.value() <=
         calculateStopDistance(behavior_parameter_.dynamic_constraints) +
-          vehicle_parameters.bounding_box.dimensions.x + 5) {
+          vehicle_parameters.bounding_box.dimensions.x + kFrontEntityStopMargin) {
         return BT::NodeStatus::FAILURE;
       }
     }
@@ -116,14 +116,14 @@ BT::NodeStatus FollowLaneAction::tick()
       if (
         distance_to_stopline.value() <=
         calculateStopDistance(behavior_parameter_.dynamic_constraints) +
-          vehicle_parameters.bounding_box.dimensions.x * 0.5 + 5) {
+          vehicle_parameters.bounding_box.dimensions.x * kBoundingBoxHalfFactor + kStopLineMargin) {
         return BT::NodeStatus::FAILURE;
       }
     }
     if (distance_to_conflicting_entity) {
       if (
         distance_to_conflicting_entity.value() <
-        (vehicle_parameters.bounding_box.dimensions.x + 3 +
+        (vehicle_parameters.bounding_box.dimensions.x + kConflictingEntityMargin +
          calculateStopDistance(behavior_parameter_.dynamic_constraints))) {
         return BT::NodeStatus::FAILURE;
       }

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

@@ -31,7 +31,6 @@ StopAtCrossingEntityAction::StopAtCrossingEntityAction(
   const std::string & name, const BT::NodeConfiguration & config)
 : entity_behavior::VehicleActionNode(name, config)
 {
-  in_stop_sequence_ = false;
 }
 
 const std::optional<traffic_simulator_msgs::msg::Obstacle>
@@ -89,19 +88,15 @@ BT::NodeStatus StopAtCrossingEntityAction::tick()
   if (
     request_ != traffic_simulator::behavior::Request::NONE &&
     request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
-    in_stop_sequence_ = false;
     return BT::NodeStatus::FAILURE;
   }
   if (!canonicalized_entity_status_->isInLanelet()) {
-    in_stop_sequence_ = false;
     return BT::NodeStatus::FAILURE;
   }
   if (!behavior_parameter_.see_around) {
-    in_stop_sequence_ = false;
     return BT::NodeStatus::FAILURE;
   }
   if (getRightOfWayEntities(route_lanelets_).size() != 0) {
-    in_stop_sequence_ = false;
     return BT::NodeStatus::FAILURE;
   }
   const auto waypoints = calculateWaypoints();
@@ -111,39 +106,26 @@ BT::NodeStatus StopAtCrossingEntityAction::tick()
   if (trajectory == nullptr) {
     return BT::NodeStatus::FAILURE;
   }
-  distance_to_stop_target_ = getDistanceToConflictingEntity(route_lanelets_, *trajectory);
+  std::optional<double> target_linear_speed;
   auto distance_to_stopline =
     traffic_simulator::distance::distanceToStopLine(route_lanelets_, *trajectory);
   const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory);
-  if (!distance_to_stop_target_) {
-    in_stop_sequence_ = false;
+  distance_to_stop_target_ = getDistanceToConflictingEntity(route_lanelets_, *trajectory);
+  if (distance_to_stop_target_) {
+    target_linear_speed = calculateTargetSpeed(canonicalized_entity_status_->getTwist().linear.x);
+  } else {
     return BT::NodeStatus::FAILURE;
   }
   if (distance_to_front_entity) {
     if (distance_to_front_entity.value() <= distance_to_stop_target_.value()) {
-      in_stop_sequence_ = false;
       return BT::NodeStatus::FAILURE;
     }
   }
   if (distance_to_stopline) {
     if (distance_to_stopline.value() <= distance_to_stop_target_.value()) {
-      in_stop_sequence_ = false;
       return BT::NodeStatus::FAILURE;
     }
   }
-  std::optional<double> target_linear_speed;
-  if (distance_to_stop_target_) {
-    target_linear_speed = calculateTargetSpeed(canonicalized_entity_status_->getTwist().linear.x);
-  } else {
-    target_linear_speed = std::nullopt;
-  }
-  if (!distance_to_stop_target_) {
-    setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(0));
-    setOutput("waypoints", waypoints);
-    setOutput("obstacle", calculateObstacle(waypoints));
-    in_stop_sequence_ = false;
-    return BT::NodeStatus::SUCCESS;
-  }
   if (target_speed_) {
     if (target_speed_.value() > target_linear_speed.value()) {
       target_speed_ = target_linear_speed.value();
@@ -154,7 +136,6 @@ BT::NodeStatus StopAtCrossingEntityAction::tick()
   setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value()));
   setOutput("waypoints", waypoints);
   setOutput("obstacle", calculateObstacle(waypoints));
-  in_stop_sequence_ = true;
   return BT::NodeStatus::RUNNING;
 }
 }  // namespace follow_lane_sequence

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

@@ -30,7 +30,6 @@ StopAtStopLineAction::StopAtStopLineAction(
   const std::string & name, const BT::NodeConfiguration & config)
 : entity_behavior::VehicleActionNode(name, config)
 {
-  stopped_ = false;
 }
 
 const std::optional<traffic_simulator_msgs::msg::Obstacle> StopAtStopLineAction::calculateObstacle(
@@ -53,15 +52,12 @@ const std::optional<traffic_simulator_msgs::msg::Obstacle> StopAtStopLineAction:
 
 const traffic_simulator_msgs::msg::WaypointsArray StopAtStopLineAction::calculateWaypoints()
 {
-  if (!canonicalized_entity_status_->isInLanelet()) {
-    THROW_SIMULATION_ERROR("failed to assign lane");
-  }
   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();
     waypoints.waypoints = reference_trajectory->getTrajectory(
-      lanelet_pose.s, lanelet_pose.s + horizon, 1.0, lanelet_pose.offset);
+      lanelet_pose.s, lanelet_pose.s + horizon, kWaypointInterval, lanelet_pose.offset);
     trajectory = std::make_unique<math::geometry::CatmullRomSubspline>(
       reference_trajectory, lanelet_pose.s, lanelet_pose.s + horizon);
     return waypoints;
@@ -79,7 +75,8 @@ std::optional<double> StopAtStopLineAction::calculateTargetSpeed(double current_
    * @brief hard coded parameter!! 1.0 is a stop margin
    */
   double rest_distance =
-    distance_to_stopline_.value() - (vehicle_parameters.bounding_box.dimensions.x * 0.5 + 1.0);
+    distance_to_stopline_.value() -
+    (vehicle_parameters.bounding_box.dimensions.x * kBoundingBoxHalfFactor + kStopMargin);
   if (rest_distance < calculateStopDistance(behavior_parameter_.dynamic_constraints)) {
     return 0;
   }
@@ -92,7 +89,6 @@ BT::NodeStatus StopAtStopLineAction::tick()
   if (
     request_ != traffic_simulator::behavior::Request::NONE &&
     request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) {
-    stopped_ = false;
     return BT::NodeStatus::FAILURE;
   }
   if (!behavior_parameter_.see_around) {
@@ -101,6 +97,9 @@ BT::NodeStatus StopAtStopLineAction::tick()
   if (getRightOfWayEntities(route_lanelets_).size() != 0) {
     return BT::NodeStatus::FAILURE;
   }
+  if (!canonicalized_entity_status_->isInLanelet()) {
+    return BT::NodeStatus::FAILURE;
+  }
   const auto waypoints = calculateWaypoints();
   if (waypoints.waypoints.empty()) {
     return BT::NodeStatus::FAILURE;
@@ -113,49 +112,37 @@ BT::NodeStatus StopAtStopLineAction::tick()
   const auto distance_to_stop_target = getDistanceToConflictingEntity(route_lanelets_, *trajectory);
   const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory);
   if (!distance_to_stopline_) {
-    stopped_ = false;
     return BT::NodeStatus::FAILURE;
   }
   if (distance_to_front_entity) {
     if (distance_to_front_entity.value() <= distance_to_stopline_.value()) {
-      stopped_ = false;
       return BT::NodeStatus::FAILURE;
     }
   }
   if (distance_to_stop_target) {
     if (distance_to_stop_target.value() <= distance_to_stopline_.value()) {
-      stopped_ = false;
       return BT::NodeStatus::FAILURE;
     }
   }
 
-  if (std::fabs(canonicalized_entity_status_->getTwist().linear.x) < 0.001) {
+  if (std::fabs(canonicalized_entity_status_->getTwist().linear.x) < kVelocityEpsilon) {
     if (distance_to_stopline_) {
-      if (distance_to_stopline_.value() <= vehicle_parameters.bounding_box.dimensions.x + 5) {
-        stopped_ = true;
+      if (
+        distance_to_stopline_.value() <=
+        vehicle_parameters.bounding_box.dimensions.x + kFrontStoplineMargin) {
+        if (!target_speed_) {
+          target_speed_ = hdmap_utils_->getSpeedLimit(route_lanelets_);
+        }
+        setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value()));
+        setOutput("waypoints", waypoints);
+        setOutput("obstacle", calculateObstacle(waypoints));
+        return BT::NodeStatus::RUNNING;
       }
     }
   }
-  if (stopped_) {
-    if (!target_speed_) {
-      target_speed_ = hdmap_utils_->getSpeedLimit(route_lanelets_);
-    }
-    if (!distance_to_stopline_) {
-      stopped_ = false;
-      setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value()));
-      setOutput("waypoints", waypoints);
-      setOutput("obstacle", calculateObstacle(waypoints));
-      return BT::NodeStatus::SUCCESS;
-    }
-    setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value()));
-    setOutput("waypoints", waypoints);
-    setOutput("obstacle", calculateObstacle(waypoints));
-    return BT::NodeStatus::RUNNING;
-  }
   auto target_linear_speed =
     calculateTargetSpeed(canonicalized_entity_status_->getTwist().linear.x);
   if (!target_linear_speed) {
-    stopped_ = false;
     return BT::NodeStatus::FAILURE;
   }
   if (target_speed_) {
@@ -166,7 +153,6 @@ BT::NodeStatus StopAtStopLineAction::tick()
     target_speed_ = target_linear_speed.value();
   }
   setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value()));
-  stopped_ = false;
   setOutput("waypoints", waypoints);
   setOutput("obstacle", calculateObstacle(waypoints));
   return BT::NodeStatus::RUNNING;

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

@@ -118,12 +118,6 @@ BT::NodeStatus StopAtTrafficLightAction::tick()
   } else {
     return BT::NodeStatus::FAILURE;
   }
-  if (!distance_to_stop_target_) {
-    setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(0));
-    setOutput("waypoints", waypoints);
-    setOutput("obstacle", calculateObstacle(waypoints));
-    return BT::NodeStatus::SUCCESS;
-  }
   if (target_speed_) {
     if (target_speed_.value() > target_linear_speed.value()) {
       target_speed_ = target_linear_speed.value();

simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp

@@ -67,8 +67,7 @@ const traffic_simulator_msgs::msg::WaypointsArray LaneChangeAction::calculateWay
       waypoints.waypoints = straight_waypoints;
       const auto curve_waypoints = curve_->getTrajectory(current_s_, l, 1.0, true);
       std::copy(
-        straight_waypoints.begin(), straight_waypoints.end(),
-        std::back_inserter(waypoints.waypoints));
+        curve_waypoints.begin(), curve_waypoints.end(), std::back_inserter(waypoints.waypoints));
     }
     return waypoints;
   } else {
@@ -171,6 +170,9 @@ BT::NodeStatus LaneChangeAction::tick()
             lane_change_velocity_ = curve_->getLength() / lane_change_parameters_->constraint.value;
             break;
         }
+        if (target_speed_) {
+          lane_change_velocity_ = std::clamp(lane_change_velocity_, 0.0, target_speed_.value());
+        }
       } else {
         return BT::NodeStatus::FAILURE;
       }