feat(cruise_planner,planning_evaluator): add cruise and slow down diags (autowarefoundation#7960)

* add cruise and slow down diags to cruise planner

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add cruise types

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* adjust planning eval

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp

@@ -181,7 +181,9 @@ class PlanningEvaluatorNode : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr timer_;
   // queue for diagnostics and time stamp
   std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
-  const std::vector<std::string> target_functions_ = {"obstacle_cruise_planner"};
+  const std::vector<std::string> target_functions_ = {
+    "obstacle_cruise_planner_stop", "obstacle_cruise_planner_slow_down",
+    "obstacle_cruise_planner_cruise"};
   std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
 };
 }  // namespace planning_diagnostics

evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp

@@ -122,7 +122,21 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticEvaluationStatus(
   });
   const bool found = it != diag.values.end();
   status.level = (found) ? status.OK : status.ERROR;
-  status.values.push_back((found) ? *it : diagnostic_msgs::msg::KeyValue{});
+
+  auto get_key_value = [&]() {
+    if (!found) {
+      return diagnostic_msgs::msg::KeyValue{};
+    }
+    if (it->value.find("none") != std::string::npos) {
+      return *it;
+    }
+    diagnostic_msgs::msg::KeyValue key_value;
+    key_value.key = "decision";
+    key_value.value = "deceleration";
+    return key_value;
+  };
+
+  status.values.push_back(get_key_value());
   return status;
 }
 

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp

@@ -25,6 +25,8 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
 #include <optional>
 #include <string>
 #include <vector>
@@ -286,6 +288,9 @@ struct DebugData
   MarkerArray cruise_wall_marker;
   MarkerArray slow_down_wall_marker;
   std::vector<autoware::universe_utils::Polygon2d> detection_polygons;
+  std::optional<diagnostic_msgs::msg::DiagnosticStatus> stop_reason_diag{std::nullopt};
+  std::optional<diagnostic_msgs::msg::DiagnosticStatus> slow_down_reason_diag{std::nullopt};
+  std::optional<diagnostic_msgs::msg::DiagnosticStatus> cruise_reason_diag{std::nullopt};
 };
 
 struct EgoNearestParam

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp

@@ -95,6 +95,14 @@ class PlannerInterface
     const std::vector<SlowDownObstacle> & slow_down_obstacles,
     std::optional<VelocityLimit> & vel_limit);
 
+  DiagnosticStatus makeEmptyDiagnostic(const std::string & reason);
+  DiagnosticStatus makeDiagnostic(
+    const std::string & reason, const std::optional<PlannerData> & planner_data = std::nullopt,
+    const std::optional<geometry_msgs::msg::Pose> & stop_pose = std::nullopt,
+    const std::optional<StopObstacle> & stop_obstacle = std::nullopt);
+  void publishDiagnostics(const rclcpp::Time & current_time);
+  void clearDiagnostics();
+
   void onParam(const std::vector<rclcpp::Parameter> & parameters)
   {
     updateCommonParam(parameters);

planning/autoware_obstacle_cruise_planner/src/node.cpp

@@ -636,6 +636,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
 
   // 8. Publish debug data
   published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp);
+  planner_ptr_->publishDiagnostics(now());
   publishDebugMarker();
   publishDebugInfo();
 

planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp

@@ -199,6 +199,7 @@ std::vector<TrajectoryPoint> OptimizationBasedPlanner::generateCruiseTrajectory(
       output.at(i).longitudinal_velocity_mps = 0.0;
     }
     prev_output_ = output;
+    debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data);
     return output;
   } else if (opt_position.size() == 1) {
     RCLCPP_DEBUG(
@@ -255,6 +256,7 @@ std::vector<TrajectoryPoint> OptimizationBasedPlanner::generateCruiseTrajectory(
   // Insert Closest Stop Point
   autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output);
 
+  debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data);
   prev_output_ = output;
   return output;
 }

planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -323,6 +323,7 @@ std::vector<TrajectoryPoint> PIDBasedPlanner::planCruise(
 
       // cruise obstacle
       debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle);
+      debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data);
     }
 
     // do cruise planning

planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp

@@ -25,6 +25,7 @@
 #include <boost/geometry/algorithms/distance.hpp>
 #include <boost/geometry/strategies/strategies.hpp>
 
+#include <optional>
 #include <string>
 
 namespace
@@ -106,78 +107,6 @@ VelocityFactorArray makeVelocityFactorArray(
   return velocity_factor_array;
 }
 
-DiagnosticArray makeEmptyStopReasonDiagnosticArray(const rclcpp::Time & current_time)
-{
-  // Create status
-  DiagnosticStatus status;
-  status.level = status.OK;
-  status.name = "obstacle_cruise_planner";
-  diagnostic_msgs::msg::KeyValue key_value;
-  {
-    // Decision
-    key_value.key = "decision";
-    key_value.value = "none";
-    status.values.push_back(key_value);
-  }
-  // create array
-  DiagnosticArray diagnostics;
-  diagnostics.header.stamp = current_time;
-  diagnostics.header.frame_id = "map";
-  diagnostics.status.push_back(status);
-  return diagnostics;
-}
-
-DiagnosticArray makeStopReasonDiagnosticArray(
-  const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose,
-  const StopObstacle & stop_obstacle)
-{
-  // Create status
-  DiagnosticStatus status;
-  status.level = status.OK;
-  status.name = "obstacle_cruise_planner";
-  diagnostic_msgs::msg::KeyValue key_value;
-  {
-    // Decision
-    key_value.key = "decision";
-    key_value.value = "stop";
-    status.values.push_back(key_value);
-  }
-
-  {  // Stop info
-    key_value.key = "stop_position";
-    const auto & p = stop_pose.position;
-    key_value.value =
-      "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}";
-    status.values.push_back(key_value);
-    key_value.key = "stop_orientation";
-    const auto & o = stop_pose.orientation;
-    key_value.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " +
-                      std::to_string(o.y) + ", " + std::to_string(o.z) + "}";
-    status.values.push_back(key_value);
-    const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength(
-      planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position);
-    key_value.key = "distance_to_stop_pose";
-    key_value.value = std::to_string(dist_to_stop_pose);
-    status.values.push_back(key_value);
-  }
-
-  {
-    // Obstacle info
-    const auto & p = stop_obstacle.collision_point;
-    key_value.key = "collision_point";
-    key_value.value =
-      "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}";
-    status.values.push_back(key_value);
-  }
-
-  // create array
-  DiagnosticArray diagnostics;
-  diagnostics.header.stamp = planner_data.current_time;
-  diagnostics.header.frame_id = "map";
-  diagnostics.status.push_back(status);
-  return diagnostics;
-}
-
 double calcMinimumDistanceToStop(
   const double initial_vel, const double max_acc, const double min_acc)
 {
@@ -319,7 +248,6 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
   if (stop_obstacles.empty()) {
     stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time));
     velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time));
-    diagnostics_pub_->publish(makeEmptyStopReasonDiagnosticArray(planner_data.current_time));
     // delete marker
     const auto markers =
       autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
@@ -474,11 +402,11 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose;
     const auto stop_reasons_msg =
       makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle);
-    const auto stop_reason_diagnostic_array =
-      makeStopReasonDiagnosticArray(planner_data, stop_pose, *determined_stop_obstacle);
     stop_reasons_pub_->publish(stop_reasons_msg);
     velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose));
-    diagnostics_pub_->publish(stop_reason_diagnostic_array);
+    // Store stop reason debug data
+    debug_data_ptr_->stop_reason_diag =
+      makeDiagnostic("stop", planner_data, stop_pose, *determined_stop_obstacle);
     // Publish if ego vehicle will over run against the stop point with a limit acceleration
 
     const bool will_over_run = determined_zero_vel_dist.value() >
@@ -747,7 +675,11 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
         markers, &debug_data_ptr_->slow_down_debug_wall_marker);
     }
 
+    // Add debug data
     debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle);
+    if (!debug_data_ptr_->slow_down_reason_diag.has_value()) {
+      debug_data_ptr_->slow_down_reason_diag = makeDiagnostic("slow_down", planner_data);
+    }
 
     // update prev_slow_down_output_
     new_prev_slow_down_output.push_back(SlowDownOutput{
@@ -904,3 +836,90 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints(
   return std::make_tuple(
     filtered_dist_to_slow_down_start, filtered_dist_to_slow_down_end, feasible_slow_down_vel);
 }
+
+DiagnosticStatus PlannerInterface::makeEmptyDiagnostic(const std::string & reason)
+{
+  // Create status
+  DiagnosticStatus status;
+  status.level = status.OK;
+  status.name = "obstacle_cruise_planner_" + reason;
+  diagnostic_msgs::msg::KeyValue key_value;
+  {
+    // Decision
+    key_value.key = "decision";
+    key_value.value = "none";
+    status.values.push_back(key_value);
+  }
+
+  return status;
+}
+
+DiagnosticStatus PlannerInterface::makeDiagnostic(
+  const std::string & reason, const std::optional<PlannerData> & planner_data,
+  const std::optional<geometry_msgs::msg::Pose> & stop_pose,
+  const std::optional<StopObstacle> & stop_obstacle)
+{
+  // Create status
+  DiagnosticStatus status;
+  status.level = status.OK;
+  status.name = "obstacle_cruise_planner_" + reason;
+  diagnostic_msgs::msg::KeyValue key_value;
+  {
+    // Decision
+    key_value.key = "decision";
+    key_value.value = reason;
+    status.values.push_back(key_value);
+  }
+
+  if (stop_pose.has_value() && planner_data.has_value()) {  // Stop info
+    key_value.key = "stop_position";
+    const auto & p = stop_pose.value().position;
+    key_value.value =
+      "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}";
+    status.values.push_back(key_value);
+    key_value.key = "stop_orientation";
+    const auto & o = stop_pose.value().orientation;
+    key_value.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " +
+                      std::to_string(o.y) + ", " + std::to_string(o.z) + "}";
+    status.values.push_back(key_value);
+    const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength(
+      planner_data.value().traj_points, planner_data.value().ego_pose.position,
+      stop_pose.value().position);
+    key_value.key = "distance_to_stop_pose";
+    key_value.value = std::to_string(dist_to_stop_pose);
+    status.values.push_back(key_value);
+  }
+
+  if (stop_obstacle.has_value()) {
+    // Obstacle info
+    const auto & p = stop_obstacle.value().collision_point;
+    key_value.key = "collision_point";
+    key_value.value =
+      "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}";
+    status.values.push_back(key_value);
+  }
+
+  return status;
+}
+
+void PlannerInterface::publishDiagnostics(const rclcpp::Time & current_time)
+{
+  // create array
+  DiagnosticArray diagnostics;
+  diagnostics.header.stamp = current_time;
+  diagnostics.header.frame_id = "map";
+  const auto & d = debug_data_ptr_;
+  diagnostics.status = {
+    (d->stop_reason_diag) ? d->stop_reason_diag.value() : makeEmptyDiagnostic("stop"),
+    (d->slow_down_reason_diag) ? *(d->slow_down_reason_diag) : makeEmptyDiagnostic("slow_down"),
+    (d->cruise_reason_diag) ? d->cruise_reason_diag.value() : makeEmptyDiagnostic("cruise")};
+  diagnostics_pub_->publish(diagnostics);
+  clearDiagnostics();
+}
+
+void PlannerInterface::clearDiagnostics()
+{
+  debug_data_ptr_->stop_reason_diag = std::nullopt;
+  debug_data_ptr_->slow_down_reason_diag = std::nullopt;
+  debug_data_ptr_->cruise_reason_diag = std::nullopt;
+}