feat(obstacle_cruise_planner): add slow down debug info (#3498)

* feat(obstacle_cruise_planner): add slow down debug info Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * remove unnecessary file Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Apr 22, 2023 · d04e65d · d04e65d
1 parent c7e1f82
commit d04e65d
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 2 deletions.
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -118,6 +118,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_slow_down_wall_marker_pub_;
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_stop_planning_info_pub_;
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_cruise_planning_info_pub_;
+  rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_slow_down_planning_info_pub_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_pub_;
 
   // subscriber

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
@@ -86,6 +86,11 @@ class PlannerInterface
   {
     return Float32MultiArrayStamped{};
   }
+  Float32MultiArrayStamped getSlowDownPlanningDebugMessage(const rclcpp::Time & current_time)
+  {
+    slow_down_debug_multi_array_.stamp = current_time;
+    return slow_down_debug_multi_array_;
+  }
 
 protected:
   // Parameters
@@ -113,6 +118,7 @@ class PlannerInterface
 
   // debug info
   StopPlanningDebugInfo stop_planning_debug_info_;
+  Float32MultiArrayStamped slow_down_debug_multi_array_;
 
   double calcDistanceToCollisionPoint(
     const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point);

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
@@ -334,6 +334,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
     create_publisher<Float32MultiArrayStamped>("~/debug/stop_planning_info", 1);
   debug_cruise_planning_info_pub_ =
     create_publisher<Float32MultiArrayStamped>("~/debug/cruise_planning_info", 1);
+  debug_slow_down_planning_info_pub_ =
+    create_publisher<Float32MultiArrayStamped>("~/debug/slow_down_planning_info", 1);
 
   const auto longitudinal_info = LongitudinalInfo(*this);
 
@@ -1287,8 +1289,8 @@ void ObstacleCruisePlannerNode::publishDebugInfo() const
   debug_cruise_planning_info_pub_->publish(cruise_debug_msg);
 
   // slow_down
-  // const auto slow_down_debug_msg = planner_ptr_->getSlowDownPlanningDebugMessage(now());
-  // debug_slow_down_planning_info_pub_->publish(slow_down_debug_msg);
+  const auto slow_down_debug_msg = planner_ptr_->getSlowDownPlanningDebugMessage(now());
+  debug_slow_down_planning_info_pub_->publish(slow_down_debug_msg);
 }
 
 void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const

diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -282,6 +282,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
 {
   stop_watch_.tic(__func__);
   auto slow_down_traj_points = cruise_traj_points;
+  slow_down_debug_multi_array_ = Float32MultiArrayStamped();
 
   const double dist_to_ego = [&]() {
     const size_t ego_seg_idx =
@@ -329,6 +330,9 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
     if (!slow_down_start_idx) {
       continue;
     }
+    slow_down_debug_multi_array_.data.push_back(obstacle.precise_lat_dist);
+    slow_down_debug_multi_array_.data.push_back(slow_down_vel);
+    slow_down_debug_multi_array_.data.push_back(*slow_down_start_idx);
 
     // calculate slow down end distance, and insert slow down velocity
     const double dist_to_slow_down_end =
@@ -346,6 +350,9 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
       for (size_t i = *slow_down_start_idx; i <= *slow_down_end_idx; ++i) {
         slow_down_traj_points.at(i).longitudinal_velocity_mps = slow_down_vel;
       }
+      slow_down_debug_multi_array_.data.push_back(*slow_down_end_idx);
+    } else {
+      slow_down_debug_multi_array_.data.push_back(-1.0);  // push back invalid value
     }
 
     debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle);