Added conditional timer and logging\n

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

Author: PanConChicharron

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp

@@ -177,7 +177,7 @@ class MPTOptimizer
     const std::shared_ptr<DebugData> debug_data_ptr,
     const std::shared_ptr<TimeKeeper> time_keeper_ptr);
 
-  std::vector<TrajectoryPoint> optimizeTrajectory(
+  std::pair<std::vector<TrajectoryPoint>, int> optimizeTrajectory(
     const PlannerData & planner_data, const std::vector<TrajectoryPoint> & smoothed_points);
   std::optional<std::vector<TrajectoryPoint>> getPrevOptimizedTrajectoryPoints() const;
 

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -20,6 +20,7 @@
 #include "obstacle_avoidance_planner/mpt_optimizer.hpp"
 #include "obstacle_avoidance_planner/replan_checker.hpp"
 #include "obstacle_avoidance_planner/type_alias.hpp"
+#include "obstacle_avoidance_planner/utils/conditional_timer.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
@@ -59,6 +60,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
   vehicle_info_util::VehicleInfo vehicle_info_{};
   mutable std::shared_ptr<DebugData> debug_data_ptr_{nullptr};
   mutable std::shared_ptr<TimeKeeper> time_keeper_ptr_{nullptr};
+  mutable std::shared_ptr<ConditionalTimer> conditional_timer_{nullptr};
 
   // flags for some functions
   bool enable_pub_debug_marker_;

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/conditional_timer.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <chrono>
+#include <optional>
+
+class ConditionalTimer {
+public:
+    void update(bool condition) {
+        if (condition && !start_time_.has_value()) {
+            // Condition met, start the timer
+            start_time_ = std::chrono::high_resolution_clock::now();
+        } else if (!condition && start_time_.has_value()) {
+            // Condition no longer met, stop the timer
+            start_time_ = std::nullopt;
+        }
+    }
+
+    std::chrono::duration<double> getElapsedTime() const {
+        if (start_time_.has_value()) {
+            auto current_time = std::chrono::high_resolution_clock::now();
+            return current_time - *start_time_;
+        } else {
+            return std::chrono::duration<double>(0.0);;
+        }
+    }
+
+private:
+    std::optional<std::chrono::time_point<std::chrono::high_resolution_clock>> start_time_{std::nullopt};
+};

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -473,15 +473,15 @@ void MPTOptimizer::onParam(const std::vector<rclcpp::Parameter> & parameters)
   debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num;
 }
 
-std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(
+std::pair<std::vector<TrajectoryPoint>, int> MPTOptimizer::optimizeTrajectory(
   const PlannerData & planner_data, const std::vector<TrajectoryPoint> & smoothed_points)
 {
   time_keeper_ptr_->tic(__func__);
 
   const auto & p = planner_data;
   const auto & traj_points = p.traj_points;
 
-  const auto get_prev_optimized_traj_points = [&]() {
+  auto get_prev_optimized_traj_points = [&]() {
     if (prev_optimized_traj_points_ptr_) {
       return *prev_optimized_traj_points_ptr_;
     }
@@ -493,7 +493,7 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(
   if (ref_points.size() < 2) {
     RCLCPP_INFO_EXPRESSION(
       logger_, enable_debug_info_, "return std::nullopt since ref_points size is less than 2.");
-    return get_prev_optimized_traj_points();
+    return std::make_pair<std::vector<TrajectoryPoint>, int>(get_prev_optimized_traj_points(), -1);
   }
 
   // 2. calculate B and W matrices where x = B u + W
@@ -528,14 +528,14 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(
     RCLCPP_INFO_STREAM(logger_, "obj_mat: " << obj_mat);
     RCLCPP_INFO_STREAM(logger_, "const_mat: " << const_mat);
 
-    return get_prev_optimized_traj_points();
+    return std::make_pair<std::vector<TrajectoryPoint>, int>(get_prev_optimized_traj_points(), -1);
   }
 
   // 7. convert to points with validation
-  const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_variables, mpt_mat);
+  auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_variables, mpt_mat);
   if (!mpt_traj_points) {
     RCLCPP_WARN(logger_, "return std::nullopt since lateral or yaw error is too large.");
-    return get_prev_optimized_traj_points();
+    return std::make_pair<std::vector<TrajectoryPoint>, int>(get_prev_optimized_traj_points(), -1);
   }
 
   // 8. publish trajectories for debug
@@ -548,7 +548,7 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(
   prev_optimized_traj_points_ptr_ =
     std::make_shared<std::vector<TrajectoryPoint>>(*mpt_traj_points);
 
-  return *mpt_traj_points;
+  return std::make_pair<std::vector<TrajectoryPoint>, int>(std::move(*mpt_traj_points), 1);
 }
 
 std::optional<std::vector<TrajectoryPoint>> MPTOptimizer::getPrevOptimizedTrajectoryPoints() const

planning/obstacle_avoidance_planner/src/node.cpp

@@ -76,7 +76,8 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
 : Node("obstacle_avoidance_planner", node_options),
   vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()),
   debug_data_ptr_(std::make_shared<DebugData>()),
-  time_keeper_ptr_(std::make_shared<TimeKeeper>())
+  time_keeper_ptr_(std::make_shared<TimeKeeper>()),
+  conditional_timer_(std::make_shared<ConditionalTimer>())
 {
   // interface publisher
   traj_pub_ = create_publisher<Trajectory>("~/output/path", 1);
@@ -300,22 +301,15 @@ std::vector<TrajectoryPoint> ObstacleAvoidancePlanner::generateOptimizedTrajecto
 {
   time_keeper_ptr_->tic(__func__);
 
-  const auto & input_traj_points = planner_data.traj_points;
-
   // 1. calculate trajectory with MPT
   //    NOTE: This function may return previously optimized trajectory points.
   //          Also, velocity on some points will not be updated for a logic purpose.
   auto optimized_traj_points = optimizeTrajectory(planner_data);
 
-  // 2. update velocity
-  //    NOTE: When optimization failed or is skipped, velocity in trajectory points must
-  //          be updated since velocity in input trajectory (path) may change.
-  applyInputVelocity(optimized_traj_points, input_traj_points, planner_data.ego_pose); // within 3s period
-
-  // 3. insert zero velocity when trajectory is over drivable area
+  // 2. insert zero velocity when trajectory is over drivable area
   insertZeroVelocityOutsideDrivableArea(planner_data, optimized_traj_points);
 
-  // 4. publish debug marker
+  // 3. publish debug marker
   publishDebugMarkerOfOptimization(optimized_traj_points);
 
   time_keeper_ptr_->toc(__func__, " ");
@@ -326,7 +320,8 @@ std::vector<TrajectoryPoint> ObstacleAvoidancePlanner::optimizeTrajectory(
   const PlannerData & planner_data)
 {
   time_keeper_ptr_->tic(__func__);
-  const auto & p = planner_data;
+
+  const auto & input_traj_points = planner_data.traj_points;
 
   // 1. check if replan (= optimization) is required
   const bool is_replan_required = [&]() {
@@ -341,19 +336,37 @@ std::vector<TrajectoryPoint> ObstacleAvoidancePlanner::optimizeTrajectory(
   }();
   replan_checker_ptr_->updateData(planner_data, is_replan_required, now());
   if (!is_replan_required) {
-    return getPrevOptimizedTrajectory(p.traj_points);
+    return getPrevOptimizedTrajectory(input_traj_points);
   }
 
   if (enable_skip_optimization_) {
-    return p.traj_points;
+    return input_traj_points;
   }
 
   // 2. make trajectory kinematically-feasible and collision-free (= inside the drivable area)
   //    with model predictive trajectory
-  const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data, p.traj_points);
+  const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data, input_traj_points);
+
+  const int optimized_traj_status = mpt_traj.second;
+  const bool optimized_traj_failed = (optimized_traj_status == -1);
+
+  conditional_timer_->update(optimized_traj_failed);
+
+  const double elapsed_time = conditional_timer_->getElapsedTime().count();
+  const bool elapsed_time_over_three_seconds = (elapsed_time > 3.0);
+
+  auto optimized_traj_points = std::move(optimized_traj_failed && elapsed_time_over_three_seconds ? input_traj_points : mpt_traj.first);
+  const bool apply_input_velocity = optimized_traj_failed && elapsed_time_over_three_seconds;
+
+  if (!apply_input_velocity) {
+    // 3. update velocity
+    //    NOTE: When optimization failed or is skipped, velocity in trajectory points must
+    //          be updated since velocity in input trajectory (path) may change.
+    applyInputVelocity(optimized_traj_points, input_traj_points, planner_data.ego_pose);
+  }
 
   time_keeper_ptr_->toc(__func__, "    ");
-  return mpt_traj;
+  return optimized_traj_points;
 }
 
 std::vector<TrajectoryPoint> ObstacleAvoidancePlanner::getPrevOptimizedTrajectory(