Replaced hardcoded values with named constants.

Author: takanotaiga

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_stop_line_action.hpp

@@ -43,6 +43,11 @@ class StopAtStopLineAction : public entity_behavior::VehicleActionNode
 
 private:
   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_stop_line_action.cpp

@@ -57,7 +57,7 @@ const traffic_simulator_msgs::msg::WaypointsArray StopAtStopLineAction::calculat
     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;
@@ -75,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;
   }
@@ -124,9 +125,11 @@ BT::NodeStatus StopAtStopLineAction::tick()
     }
   }
 
-  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) {
+      if (
+        distance_to_stopline_.value() <=
+        vehicle_parameters.bounding_box.dimensions.x + kFrontStoplineMargin) {
         if (!target_speed_) {
           target_speed_ = hdmap_utils_->getSpeedLimit(route_lanelets_);
         }