feat(obstacle_cruise_planner): delete shape from target obstacle (#1558)

* delete is on ego traj Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * feat(obstacle_cruise_planner): delete shape Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * remove unnecessary parameters Signed-off-by: yutaka <purewater0901@gmail.com> * add new calc distance Signed-off-by: yutaka <purewater0901@gmail.com> * add threshold Signed-off-by: yutaka <purewater0901@gmail.com> * fix a bug Signed-off-by: yutaka <purewater0901@gmail.com> * fix terminal point Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * update parameters Signed-off-by: yutaka <purewater0901@gmail.com> Signed-off-by: yutaka <purewater0901@gmail.com>
autowarefoundation · Aug 15, 2022 · 30ddff0 · 30ddff0
1 parent 319d15e
commit 30ddff0
Show file tree

Hide file tree

Showing 17 changed files with 182 additions and 1,001 deletions.
diff --git a/...g/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/...g/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
@@ -86,13 +86,11 @@
       lpf_cruise_gain: 0.2
 
     optimization_based_planner:
-      resampling_s_interval: 2.0
-      dense_resampling_time_interval: 0.1
-      sparse_resampling_time_interval: 1.0
+      dense_resampling_time_interval: 0.2
+      sparse_resampling_time_interval: 2.0
       dense_time_horizon: 5.0
       max_time_horizon: 25.0
-      max_trajectory_length: 200.0
-      velocity_margin: 0.1 #[m/s]
+      velocity_margin: 0.2 #[m/s]
 
       # Parameters for safe distance
       t_dangerous: 0.5

diff --git a/planning/obstacle_cruise_planner/CMakeLists.txt b/planning/obstacle_cruise_planner/CMakeLists.txt
@@ -10,8 +10,6 @@ ament_auto_add_library(obstacle_cruise_planner_core SHARED
   src/node.cpp
   src/utils.cpp
   src/polygon_utils.cpp
-  src/optimization_based_planner/resample.cpp
-  src/optimization_based_planner/box2d.cpp
   src/optimization_based_planner/velocity_optimizer.cpp
   src/optimization_based_planner/optimization_based_planner.cpp
   src/pid_based_planner/pid_based_planner.cpp

diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -86,13 +86,11 @@
       lpf_cruise_gain: 0.2
 
     optimization_based_planner:
-      resampling_s_interval: 2.0
-      dense_resampling_time_interval: 0.1
-      sparse_resampling_time_interval: 1.0
+      dense_resampling_time_interval: 0.2
+      sparse_resampling_time_interval: 2.0
       dense_time_horizon: 5.0
       max_time_horizon: 25.0
-      max_trajectory_length: 200.0
-      velocity_margin: 0.1 #[m/s]
+      velocity_margin: 0.2 #[m/s]
 
       # Parameters for safe distance
       t_dangerous: 0.5

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
@@ -60,7 +60,6 @@ struct TargetObstacle
     velocity = aligned_velocity;
     is_classified = true;
     classification = object.classification.at(0);
-    shape = object.shape;
     uuid = toHexString(object.object_id);
 
     predicted_paths.clear();
@@ -79,7 +78,6 @@ struct TargetObstacle
   float velocity;
   bool is_classified;
   ObjectClassification classification;
-  Shape shape;
   std::string uuid;
   std::vector<PredictedPath> predicted_paths;
   std::vector<geometry_msgs::msg::PointStamped> collision_points;

diff --git a/...tacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/box2d.hpp b/...tacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/box2d.hpp
diff --git a/...include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/...include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp
@@ -15,9 +15,7 @@
 #ifndef OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_
 #define OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_
 
-#include "obstacle_cruise_planner/optimization_based_planner/box2d.hpp"
 #include "obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp"
-#include "obstacle_cruise_planner/optimization_based_planner/st_point.hpp"
 #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp"
 #include "obstacle_cruise_planner/planner_interface.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
@@ -53,85 +51,47 @@ class OptimizationBasedPlanner : public PlannerInterface
     DebugData & debug_data) override;
 
 private:
-  struct TrajectoryData
-  {
-    TrajectoryData() {}
-
-    Trajectory traj;
-    std::vector<double> s;
-  };
-
-  struct ObjectData
-  {
-    geometry_msgs::msg::Pose pose;
-    double length;
-    double width;
-    double time;
-  };
-
   // Member Functions
   std::vector<double> createTimeVector();
   std::tuple<double, double> calcInitialMotion(
-    const ObstacleCruisePlannerData & planner_data, const size_t input_closest,
-    const Trajectory & prev_traj);
+    const ObstacleCruisePlannerData & planner_data, const Trajectory & prev_traj);
 
   TrajectoryPoint calcInterpolatedTrajectoryPoint(
     const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose);
   bool checkHasReachedGoal(const ObstacleCruisePlannerData & planner_data);
-  TrajectoryData getTrajectoryData(
-    const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose);
-
-  TrajectoryData resampleTrajectoryData(
-    const TrajectoryData & base_traj_data, const double resampling_s_interval,
-    const double max_traj_length);
-  Trajectory resampleTrajectory(
-    const std::vector<double> & base_index, const Trajectory & base_trajectory,
-    const std::vector<double> & query_index, const bool use_spline_for_pose = false);
 
   boost::optional<SBoundaries> getSBoundaries(
-    const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data,
-    const std::vector<double> & time_vec);
+    const ObstacleCruisePlannerData & planner_data, const std::vector<double> & time_vec);
 
   boost::optional<SBoundaries> getSBoundaries(
-    const rclcpp::Time & current_time, const TrajectoryData & ego_traj_data,
-    const TargetObstacle & object, const rclcpp::Time & obj_base_time,
-    const std::vector<double> & time_vec);
+    const ObstacleCruisePlannerData & planner_data, const TargetObstacle & object,
+    const std::vector<double> & time_vec, const double traj_length);
 
-  boost::optional<SBoundaries> getSBoundaries(
-    const TrajectoryData & ego_traj_data, const std::vector<double> & time_vec,
-    const double safety_distance, const TargetObstacle & object,
-    const double dist_to_collision_point);
-
-  boost::optional<SBoundaries> getSBoundaries(
-    const rclcpp::Time & current_time, const TrajectoryData & ego_traj_data,
-    const std::vector<double> & time_vec, const double safety_distance,
-    const TargetObstacle & object, const rclcpp::Time & obj_base_time,
-    const PredictedPath & predicted_path);
+  boost::optional<SBoundaries> getSBoundariesForOnTrajectoryObject(
+    const ObstacleCruisePlannerData & planner_data, const std::vector<double> & time_vec,
+    const double safety_distance, const TargetObstacle & object, const double traj_length);
 
-  boost::optional<PredictedPath> resamplePredictedPath(
-    const TargetObstacle & object, const rclcpp::Time & obj_base_time,
-    const rclcpp::Time & current_time, const std::vector<double> & resolutions,
-    const double horizon);
+  boost::optional<SBoundaries> getSBoundariesForOffTrajectoryObject(
+    const ObstacleCruisePlannerData & planner_data, const std::vector<double> & time_vec,
+    const double safety_distance, const TargetObstacle & object, const double traj_length);
 
-  boost::optional<double> getDistanceToCollisionPoint(
-    const TrajectoryData & ego_traj_data, const ObjectData & obj_data);
+  bool checkOnTrajectory(
+    const ObstacleCruisePlannerData & planner_data, const geometry_msgs::msg::PointStamped & point);
 
-  boost::optional<size_t> getCollisionIdx(
-    const TrajectoryData & ego_traj, const Box2d & obj_box, const size_t start_idx,
-    const size_t end_idx);
+  boost::optional<double> calcTrajectoryLengthFromCurrentPose(
+    const autoware_auto_planning_msgs::msg::Trajectory & traj,
+    const geometry_msgs::msg::Pose & current_pose);
 
   geometry_msgs::msg::Pose transformBaseLink2Center(
     const geometry_msgs::msg::Pose & pose_base_link);
 
   boost::optional<VelocityOptimizer::OptimizationResult> processOptimizedResult(
-    const double v0, const VelocityOptimizer::OptimizationResult & opt_result);
+    const double v0, const VelocityOptimizer::OptimizationResult & opt_result, const double offset);
 
   void publishDebugTrajectory(
-    const rclcpp::Time & current_time, const Trajectory & traj, const size_t closest_idx,
+    const ObstacleCruisePlannerData & planner_data, const double offset,
     const std::vector<double> & time_vec, const SBoundaries & s_boundaries,
     const VelocityOptimizer::OptimizationResult & opt_result);
-  // Calculation time watcher
-  tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
 
   Trajectory prev_output_;
 
@@ -142,12 +102,9 @@ class OptimizationBasedPlanner : public PlannerInterface
   rclcpp::Publisher<Trajectory>::SharedPtr boundary_pub_;
   rclcpp::Publisher<Trajectory>::SharedPtr optimized_sv_pub_;
   rclcpp::Publisher<Trajectory>::SharedPtr optimized_st_graph_pub_;
-  rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_wall_marker_pub_;
 
   // Resampling Parameter
-  double resampling_s_interval_;
-  double max_trajectory_length_;
   double dense_resampling_time_interval_;
   double sparse_resampling_time_interval_;
   double dense_time_horizon_;

diff --git a/...le_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/resample.hpp b/...le_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/resample.hpp