fix(obstacle_avoidance_planner): apply dynamic path length to fixed t…

…rajectory in eb (autowarefoundation#2357) * fix(obstacle_avoidance_planner): apply dynamic path length to fixed trajectory in eb Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add flag to enable clipping fixed trajectory Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add maintainer Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: kminoda <koji.minoda@tier4.jp>
kminoda · Jan 6, 2023 · 3b22021 · 3b22021
1 parent eec65ac
commit 3b22021
Show file tree

Hide file tree

Showing 6 changed files with 24 additions and 1 deletion.
diff --git a/..._driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/..._driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml
@@ -39,6 +39,7 @@
       num_fix_points_for_extending: 50 # number of fixing points when extending
       max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m]
 
+      enable_clipping_fixed_traj: false
       non_fixed_trajectory_length: 5.0 # length of the trajectory merging optimized mpt trajectory to original(not optimized) trajectory
 
     object:  # avoiding object

diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml
@@ -39,6 +39,7 @@
       num_fix_points_for_extending: 50 # number of fixing points when extending
       max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m]
 
+      enable_clipping_fixed_traj: false
       non_fixed_trajectory_length: 5.0 # length of the trajectory merging optimized mpt trajectory to original(not optimized) trajectory
 
     object:  # avoiding object

diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp
@@ -210,6 +210,7 @@ struct TrajectoryParam
 
   double ego_nearest_dist_threshold;
   double ego_nearest_yaw_threshold;
+  bool enable_clipping_fixed_traj;
 };
 
 struct MPTParam

diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml
@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>The obstacle_avoidance_planner package</description>
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="takayuki.murooka@tier4.jp">Takayuki Murooka</author>

diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp
@@ -135,12 +135,28 @@ EBPathOptimizer::getOptimizedTrajectory(
     candidate_points.non_fixed_points.end());
 
   // interpolate points for logic purpose
-  const std::vector<geometry_msgs::msg::Point> interpolated_points =
+  std::vector<geometry_msgs::msg::Point> interpolated_points =
     interpolation_utils::getInterpolatedPoints(full_points, eb_param_.delta_arc_length_for_eb);
   if (interpolated_points.empty()) {
     return boost::none;
   }
 
+  // clip interpolated points with the length of path
+  if (traj_param_.enable_clipping_fixed_traj) {
+    if (path.points.size() < 2) {
+      return boost::none;
+    }
+    const auto interpolated_poses =
+      points_utils::convertToPosesWithYawEstimation(interpolated_points);
+    const auto interpolated_points_end_seg_idx = motion_utils::findNearestSegmentIndex(
+      interpolated_poses, path.points.back().pose, 3.0, 0.785);
+    if (interpolated_points_end_seg_idx) {
+      interpolated_points = std::vector<geometry_msgs::msg::Point>(
+        interpolated_points.begin(),
+        interpolated_points.begin() + interpolated_points_end_seg_idx.get());
+    }
+  }
+
   debug_data.interpolated_points = interpolated_points;
   // number of optimizing points
   const int farthest_idx = std::min(

diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp
@@ -318,6 +318,9 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
     traj_param_.non_fixed_trajectory_length =
       declare_parameter<double>("common.non_fixed_trajectory_length");
 
+    traj_param_.enable_clipping_fixed_traj =
+      declare_parameter<bool>("common.enable_clipping_fixed_traj");
+
     // object
     traj_param_.max_avoiding_ego_velocity_ms =
       declare_parameter<double>("object.max_avoiding_ego_velocity_ms");