fix(autoware_obstacle_cruise_planner): fix knownConditionTrueFalse wa…

…rnings (autowarefoundation#7620) * fix(autoware_obstacle_cruise_planner): fix knownConditionTrueFalse warnings Signed-off-by: Ryuta Kambe <[email protected]> * fix Signed-off-by: Ryuta Kambe <[email protected]> --------- Signed-off-by: Ryuta Kambe <[email protected]>
tzhong518 · Jun 24, 2024 · d46a059 · d46a059
1 parent 22aaa47
commit d46a059
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 6 deletions.
diff --git a/...are_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/...are_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp
@@ -339,16 +339,21 @@ std::tuple<double, double> OptimizationBasedPlanner::calcInitialMotion(
       const auto stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint(
         input_traj_points, ego_pose, ego_nearest_param_.dist_threshold,
         ego_nearest_param_.yaw_threshold);
-      if ((stop_dist && *stop_dist > stop_dist_to_prohibit_engage_) || !stop_dist) {
-        initial_vel = engage_velocity_;
-        initial_acc = engage_acceleration_;
+      if (!stop_dist.has_value()) {
+        RCLCPP_DEBUG(
+          rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
+          "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use "
+          "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f)",
+          vehicle_speed, target_vel, engage_velocity_, engage_vel_thr);
+        return std::make_tuple(engage_velocity_, engage_acceleration_);
+      } else if (stop_dist.value() > stop_dist_to_prohibit_engage_) {
         RCLCPP_DEBUG(
           rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
           "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use "
           "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f",
           vehicle_speed, target_vel, engage_velocity_, engage_vel_thr, stop_dist.value());
-        return std::make_tuple(initial_vel, initial_acc);
-      } else if (stop_dist) {
+        return std::make_tuple(engage_velocity_, engage_acceleration_);
+      } else {
         RCLCPP_DEBUG(
           rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
           "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist.value());

diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp
@@ -637,7 +637,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
     slow_down_debug_multi_array_.data.push_back(feasible_slow_down_vel);
     slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel);
     slow_down_debug_multi_array_.data.push_back(slow_down_start_idx ? *slow_down_start_idx : -1.0);
-    slow_down_debug_multi_array_.data.push_back(slow_down_end_idx ? *slow_down_end_idx : -1.0);
+    slow_down_debug_multi_array_.data.push_back(*slow_down_end_idx);
 
     // add virtual wall
     if (slow_down_start_idx && slow_down_end_idx) {