refactor(obstacle_stop_planner): update parameter name for readability

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml

@@ -1,28 +1,37 @@
 /**:
   ros__parameters:
-    hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s]
+    hunting_threshold: 0.5                 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s]
 
     stop_planner:
-      stop_margin: 5.0 # stop margin distance from obstacle on the path [m]
-      min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m]
-      step_length: 1.0 # step length for pointcloud search range [m]
-      extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance
-      expand_stop_range: 0.0 # margin of vehicle footprint [m]
-      hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m]
+      # params for margin
+      lateral_margin: 0.0                  # margin of vehicle footprint [m]
+      max_longitudinal_margin: 5.0         # stop margin distance from obstacle on the path [m]
+      min_longitudinal_margin: 2.0         # stop margin distance when any other stop point is inserted in stop margin [m]
+
+      # params for trajectory pre-process
+      step_length: 1.0                     # step length for pointcloud search range [m]
+      extend_distance: 0.0                 # extend trajectory to consider after goal obstacle in the extend_distance
+
+      # others
+      hold_stop_margin_distance: 0.0       # the ego keeps stopping if the ego is in this margin [m]
 
     slow_down_planner:
-      # slow down planner parameters
-      forward_margin: 5.0 # margin distance from slow down point to vehicle front [m]
-      backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m]
-      expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
-      max_slow_down_vel: 1.38 # max slow down velocity [m/s]
-      min_slow_down_vel: 0.28 # min slow down velocity [m/s]
+      # params for margin
+      lateral_margin: 1.0                  # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
+      longitudinal_forward_margin: 5.0     # margin distance from slow down point to vehicle front [m]
+      longitudinal_backward_margin: 5.0    # margin distance from slow down point to vehicle rear [m]
+      longitudinal_margin_span: -0.1       # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s]
+      min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s]
+
+      # params for velocity
+      max_slow_down_velocity: 1.38         # max slow down velocity (use this param if consider_constraints is False)[m/s]
+      min_slow_down_velocity: 0.28         # min slow down velocity (use this param if consider_constraints is False)[m/s]
+      slow_down_velocity: 1.38             # target slow down velocity (use this param if consider_constraints is True)[m/s]
+
+      # params for deceleration constraints (use this param if consider_constraints is True)
+      jerk_min_slow_down: -0.3             # min slow down jerk constraint [m/sss]
+      jerk_span: -0.01                     # fineness param for planning deceleration jerk [m/sss]
+      jerk_start: -0.1                     # init jerk used for deceleration planning [m/sss]
 
-      # slow down constraint parameters
-      consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel
-      forward_margin_min: 1.0 # min margin for relaxing slow down margin [m/s]
-      forward_margin_span: -0.1 # fineness param for relaxing slow down margin [m/s]
-      jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss]
-      jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss]
-      jerk_start: -0.1 # init jerk used for deceleration planning [m/sss]
-      slow_down_vel: 1.38 # target slow down velocity [m/s]
+      # others
+      consider_constraints: False          # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

@@ -17,6 +17,7 @@
 
 #include "obstacle_stop_planner/adaptive_cruise_control.hpp"
 #include "obstacle_stop_planner/debug_marker.hpp"
+#include "obstacle_stop_planner/planner_data.hpp"
 
 #include <motion_utils/trajectory/tmp_conversion.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
@@ -76,97 +77,11 @@ using tier4_planning_msgs::msg::VelocityLimit;
 using tier4_planning_msgs::msg::VelocityLimitClearCommand;
 using vehicle_info_util::VehicleInfo;
 
-struct StopPoint
-{
-  TrajectoryPoint point{};
-  size_t index;
-};
-
-struct SlowDownSection
-{
-  TrajectoryPoint start_point{};
-  TrajectoryPoint end_point{};
-  size_t slow_down_start_idx;
-  size_t slow_down_end_idx;
-  double velocity;
-};
-
 class ObstacleStopPlannerNode : public rclcpp::Node
 {
 public:
   explicit ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options);
 
-  struct NodeParam
-  {
-    bool enable_slow_down;              // set True, slow down for obstacle beside the path
-    double max_velocity;                // max velocity [m/s]
-    double lowpass_gain;                // smoothing calculated current acceleration [-]
-    double hunting_threshold;           // keep slow down or stop state if obstacle vanished [s]
-    double ego_nearest_dist_threshold;  // dist threshold for ego's nearest index
-    double ego_nearest_yaw_threshold;   // yaw threshold for ego's nearest index
-  };
-
-  struct StopParam
-  {
-    double stop_margin;                // stop margin distance from obstacle on the path [m]
-    double min_behavior_stop_margin;   // margin distance, any other stop point is inserted [m]
-    double expand_stop_range;          // margin of vehicle footprint [m]
-    double extend_distance;            // trajectory extend_distance [m]
-    double step_length;                // step length for pointcloud search range [m]
-    double stop_search_radius;         // search radius for obstacle point cloud [m]
-    double hold_stop_margin_distance;  // keep stopping if the ego is in this margin [m]
-  };
-
-  struct SlowDownParam
-  {
-    double normal_min_jerk;         // min jerk limit for mild stop [m/sss]
-    double normal_min_acc;          // min deceleration limit for mild stop [m/ss]
-    double limit_min_jerk;          // min jerk limit [m/sss]
-    double limit_min_acc;           // min deceleration limit [m/ss]
-    double forward_margin;          // slow down margin(vehicle front -> obstacle) [m]
-    double backward_margin;         // slow down margin(obstacle vehicle rear) [m]
-    double expand_slow_down_range;  // lateral range of detection area [m]
-    double max_slow_down_vel;       // maximum speed in slow down section [m/s]
-    double min_slow_down_vel;       // minimum velocity in slow down section [m/s]
-    bool consider_constraints;      // set "True", decel point is planned under jerk/dec constraints
-    double slow_down_vel;           // target slow down velocity [m/s]
-    double forward_margin_min;      // min margin for relaxing slow down margin [m/s]
-    double forward_margin_span;     // fineness param for relaxing slow down margin [m/s]
-    double slow_down_min_jerk;      // min slow down jerk constraint [m/sss]
-    double jerk_start;              // init jerk used for deceleration planning [m/sss]
-    double jerk_span;               // fineness param for planning deceleration jerk [m/sss]
-    double vel_threshold_reset_velocity_limit_;  // velocity threshold,
-                                                 // check complete deceleration [m/s]
-    double dec_threshold_reset_velocity_limit_;  // acceleration threshold,
-                                                 // check complete deceleration [m/ss]
-    double slow_down_search_radius;  // search radius for slow down obstacle point cloud [m]
-  };
-
-  struct PlannerData
-  {
-    diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag{};
-
-    geometry_msgs::msg::Pose current_pose{};
-
-    pcl::PointXYZ nearest_collision_point;
-    pcl::PointXYZ nearest_slow_down_point;
-    pcl::PointXYZ lateral_nearest_slow_down_point;
-    rclcpp::Time nearest_collision_point_time{};
-    double lateral_deviation{0.0};
-
-    size_t trajectory_trim_index{};
-    size_t decimate_trajectory_collision_index{};
-    size_t decimate_trajectory_slow_down_index{};
-    std::map<size_t, size_t> decimate_trajectory_index_map{};  // key: decimate index
-                                                               // value: original index
-
-    bool found_collision_points{false};
-    bool found_slow_down_points{false};
-    bool stop_require{false};
-    bool slow_down_require{false};
-    bool enable_adaptive_cruise{false};
-  };
-
 private:
   /*
    * ROS

planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp

@@ -0,0 +1,233 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_
+#define OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
+#include <map>
+
+namespace motion_planning
+{
+
+using diagnostic_msgs::msg::DiagnosticStatus;
+
+struct StopPoint
+{
+  TrajectoryPoint point{};
+
+  size_t index;
+};
+
+struct SlowDownSection
+{
+  TrajectoryPoint start_point{};
+
+  TrajectoryPoint end_point{};
+
+  size_t slow_down_start_idx;
+
+  size_t slow_down_end_idx;
+
+  double velocity;
+};
+
+struct NodeParam
+{
+  // set True, slow down for obstacle beside the path
+  bool enable_slow_down;
+
+  // max velocity [m/s]
+  double max_velocity;
+
+  // smoothing calculated current acceleration [-]
+  double lowpass_gain;
+
+  // keep slow down or stop state if obstacle vanished [s]
+  double hunting_threshold;
+
+  // dist threshold for ego's nearest index
+  double ego_nearest_dist_threshold;
+
+  // yaw threshold for ego's nearest index
+  double ego_nearest_yaw_threshold;
+};
+
+struct StopParam
+{
+  // =================
+  // params for margin
+  // =================
+
+  // stop margin distance from obstacle on the path [m]
+  double max_longitudinal_margin;
+
+  // margin distance, any other stop point is inserted [m]
+  double min_longitudinal_margin;
+
+  // margin of vehicle footprint [m]
+  double lateral_margin;
+
+  // =================================
+  // params for trajectory pre-process
+  // =================================
+
+  // trajectory extend_distance [m]
+  double extend_distance;
+
+  // step length for pointcloud search range [m]
+  double step_length;
+
+  // ======
+  // others
+  // ======
+
+  // search radius for obstacle point cloud [m]
+  double stop_search_radius;
+
+  // keep stopping if the ego is in this margin [m]
+  double hold_stop_margin_distance;
+};
+
+struct SlowDownParam
+{
+  // =================
+  // params for margin
+  // =================
+
+  // slow down margin(vehicle front -> obstacle) [m]
+  double longitudinal_forward_margin;
+
+  // slow down margin(obstacle vehicle rear) [m]
+  double longitudinal_backward_margin;
+
+  // lateral margin for slow down area [m]
+  double lateral_margin;
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // min margin for relaxing slow down margin [m/s]
+  double min_longitudinal_forward_margin;
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // fineness param for relaxing slow down margin [m/s]
+  double longitudinal_margin_span;
+
+  // ===================
+  // params for velocity
+  // ===================
+
+  // target slow down velocity [m/s]
+  double slow_down_velocity;
+
+  // OPTIONAL (use this param if consider_constraints is False)
+  // maximum speed in slow down section [m/s]
+  double max_slow_down_velocity;
+
+  // OPTIONAL (use this param if consider_constraints is False)
+  // minimum velocity in slow down section [m/s]
+  double min_slow_down_velocity;
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // velocity threshold whether the vehicle complete deceleration [m/s]
+  double velocity_threshold_decel_complete;
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // acceleration threshold whether the vehicle complete deceleration [m/ss]
+  double acceleration_threshold_decel_complete;
+
+  // ===================================
+  // params for deceleration constraints
+  // ===================================
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // min jerk limit for mild stop [m/sss]
+  double normal_min_jerk;
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // min deceleration limit for mild stop [m/ss]
+  double normal_min_acc;
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // min jerk limit [m/sss]
+  double limit_min_jerk;
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // min deceleration limit [m/ss]
+  double limit_min_acc;
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // min slow down jerk constraint [m/sss]
+  double slow_down_min_jerk;
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // init jerk used for deceleration planning [m/sss]
+  double jerk_start;
+
+  // OPTIONAL (use this param if consider_constraints is True)
+  // fineness param for planning deceleration jerk [m/sss]
+  double jerk_span;
+
+  // ======
+  // others
+  // ======
+
+  // set "True", decel point is planned under jerk/dec constraints
+  bool consider_constraints;
+
+  // search radius for slow down obstacle point cloud [m]
+  double slow_down_search_radius;
+};
+
+struct PlannerData
+{
+  DiagnosticStatus stop_reason_diag{};
+
+  geometry_msgs::msg::Pose current_pose{};
+
+  pcl::PointXYZ nearest_collision_point;
+
+  pcl::PointXYZ nearest_slow_down_point;
+
+  pcl::PointXYZ lateral_nearest_slow_down_point;
+
+  rclcpp::Time nearest_collision_point_time{};
+
+  double lateral_deviation{0.0};
+
+  size_t trajectory_trim_index{};
+
+  size_t decimate_trajectory_collision_index{};
+
+  size_t decimate_trajectory_slow_down_index{};
+
+  // key: decimate index, value: original index
+  std::map<size_t, size_t> decimate_trajectory_index_map{};
+
+  bool found_collision_points{false};
+
+  bool found_slow_down_points{false};
+
+  bool stop_require{false};
+
+  bool slow_down_require{false};
+
+  bool enable_adaptive_cruise{false};
+};
+
+}  // namespace motion_planning
+
+#endif  // OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_