feat(behavior_path_planner): add avoidance debug (#60)

* fix: behavior path multi thread

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* add mutex unlock

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* feat(behavior_path_planner): debug messages for failed avoidance (#694)

* feat(behavior_path_planner): debug message for avoidance

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix: make_shared for the pointers

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* chore: pre-commit

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix: remove related DEBUG_PRINT

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* chore: naming, add debug to the variable name

This is so that people can infer mutable to debug, therefore reducing the amount
of time for debugging other things

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix: object id is written as string instead of uint8x16

Also fix getUuidStr function to reflect the actual hex value.
the function is moved to avoidance_util as it better served as helper function.

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* chore: fix spelling

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix: display all 32 uuid characters

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* chore: remove unnecessary header

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* feat: support more debug message

Note: need further refactoring, due to multiple similar code, plus duplicate printing.

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix mutiplication by introducing global variables

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* header stamp to get the clock

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix: renaming the type to show clearer intent of the type

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>
Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* fix(behavior_path_planner): getAvoidanceDebugMsgArray caused crash (#828)

this is due to the assignment to pointer without guards in the function

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* feat: output avoidance debug array whenever the avoidance module is enabled (#835)

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* fix: behavior path avoidance debug

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

Author: tkimura4

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -38,6 +38,9 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <tier4_planning_msgs/msg/approval.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_factor.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 #include <tier4_planning_msgs/msg/path_change_module.hpp>
 #include <tier4_planning_msgs/msg/path_change_module_array.hpp>
 #include <tier4_planning_msgs/msg/path_change_module_id.hpp>
@@ -65,6 +68,9 @@ using geometry_msgs::msg::TwistStamped;
 using nav_msgs::msg::OccupancyGrid;
 using nav_msgs::msg::Odometry;
 using route_handler::RouteHandler;
+using tier4_planning_msgs::msg::AvoidanceDebugFactor;
+using tier4_planning_msgs::msg::AvoidanceDebugMsg;
+using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::PathChangeModule;
 using tier4_planning_msgs::msg::PathChangeModuleArray;
 using visualization_msgs::msg::MarkerArray;
@@ -159,6 +165,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 private:
   rclcpp::Publisher<OccupancyGrid>::SharedPtr debug_drivable_area_publisher_;
   rclcpp::Publisher<Path>::SharedPtr debug_path_publisher_;
+  rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_publisher_;
   void publishDebugMarker(const std::vector<MarkerArray> & debug_markers);
 };

planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp

@@ -55,6 +55,7 @@ class BehaviorTreeManager
   BehaviorModuleOutput run(const std::shared_ptr<PlannerData> & data);
   std::vector<std::shared_ptr<SceneModuleStatus>> getModulesStatus();
   std::vector<MarkerArray> getDebugMarkers();
+  AvoidanceDebugMsgArray getAvoidanceDebugMsgArray();
 
 private:
   BehaviorTreeManagerParam bt_manager_param_;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -25,6 +25,9 @@
 #include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_factor.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 
 #include <memory>
 #include <string>
@@ -154,7 +157,8 @@ class AvoidanceModule : public SceneModuleInterface
   // debug
   mutable DebugData debug_data_;
   void setDebugData(const PathShifter & shifter, const DebugData & debug);
-
+  mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_point_;
+  mutable rclcpp::Time debug_avoidance_initializer_for_shift_point_time_;
   // =====================================
   // ========= helper functions ==========
   // =====================================

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp

@@ -23,6 +23,9 @@
 #include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_factor.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 
 #include <memory>
 #include <string>
@@ -33,6 +36,11 @@ namespace behavior_path_planner
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+
+using tier4_planning_msgs::msg::AvoidanceDebugFactor;
+using tier4_planning_msgs::msg::AvoidanceDebugMsg;
+using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
+
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::PoseStamped;
@@ -288,6 +296,7 @@ struct DebugData
 
   // tmp for plot
   PathWithLaneId center_line;
+  AvoidanceDebugMsgArray avoidance_debug_msg_array;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp

@@ -57,6 +57,9 @@ void setStartData(
   AvoidPoint & ap, const double start_length, const geometry_msgs::msg::Pose & start,
   const size_t start_idx, const double start_dist);
 
+std::string getUuidStr(const ObjectData & obj);
+
+std::vector<std::string> getUuidStr(const ObjectDataArray & objs);
 }  // namespace behavior_path_planner
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -25,6 +25,9 @@
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_factor.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
+#include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 
 #include <boost/optional.hpp>
 
@@ -43,6 +46,7 @@ using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
 using route_handler::LaneChangeDirection;
 using route_handler::PullOutDirection;
 using route_handler::PullOverDirection;
+using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using visualization_msgs::msg::MarkerArray;
 using PlanResult = PathWithLaneId::SharedPtr;
 
@@ -189,13 +193,22 @@ class SceneModuleInterface
 
   MarkerArray getDebugMarker() { return debug_marker_; }
 
+  AvoidanceDebugMsgArray::SharedPtr getAvoidanceDebugMsgArray()
+  {
+    if (debug_avoidance_msg_array_ptr_) {
+      debug_avoidance_msg_array_ptr_->header.stamp = clock_->now();
+    }
+    return debug_avoidance_msg_array_ptr_;
+  }
+
 private:
   std::string name_;
   rclcpp::Logger logger_;
 
 protected:
   MarkerArray debug_marker_;
   rclcpp::Clock::SharedPtr clock_;
+  mutable AvoidanceDebugMsgArray::SharedPtr debug_avoidance_msg_array_ptr_{};
 
 public:
   ApprovalHandler approval_handler_;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -91,6 +91,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   hazard_signal_publisher_ = create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
   debug_drivable_area_publisher_ = create_publisher<OccupancyGrid>("~/debug/drivable_area", 1);
   debug_path_publisher_ = create_publisher<Path>("~/debug/path_for_visualize", 1);
+  debug_avoidance_msg_array_publisher_ =
+    create_publisher<AvoidanceDebugMsgArray>("~/debug/avoidance_debug_message_array", 1);
 
   // For remote operation
   plan_ready_publisher_ = create_publisher<PathChangeModule>("~/output/ready", 1);
@@ -535,7 +537,7 @@ void BehaviorPathPlannerNode::run()
 
   // for remote operation
   publishModuleStatus(bt_manager_->getModulesStatus(), planner_data);
-
+  debug_avoidance_msg_array_publisher_->publish(bt_manager_->getAvoidanceDebugMsgArray());
   publishDebugMarker(bt_manager_->getDebugMarkers());
 
   mutex_bt_.unlock();

planning/behavior_path_planner/src/behavior_tree_manager.cpp

@@ -124,6 +124,22 @@ std::vector<MarkerArray> BehaviorTreeManager::getDebugMarkers()
   return data;
 }
 
+AvoidanceDebugMsgArray BehaviorTreeManager::getAvoidanceDebugMsgArray()
+{
+  const auto avoidance_module = std::find_if(
+    scene_modules_.begin(), scene_modules_.end(),
+    [](const std::shared_ptr<SceneModuleInterface> & module_ptr) {
+      return module_ptr->name() == "Avoidance";
+    });
+  if (avoidance_module != scene_modules_.end()) {
+    const auto & ptr = avoidance_module->get()->getAvoidanceDebugMsgArray();
+    if (ptr) {
+      return *ptr;
+    }
+  }
+  return AvoidanceDebugMsgArray();
+}
+
 BT::NodeStatus BehaviorTreeManager::checkForceApproval(const std::string & name)
 {
   const auto & approval = current_planner_data_->approval.is_force_approved;

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -74,6 +74,11 @@ bool AvoidanceModule::isExecutionReady() const
 {
   DEBUG_PRINT("AVOIDANCE isExecutionReady");
 
+  {
+    DebugData debug;
+    static_cast<void>(calcAvoidancePlanningData(debug));
+  }
+
   if (current_state_ == BT::NodeStatus::RUNNING) {
     return true;
   }
@@ -88,7 +93,6 @@ BT::NodeStatus AvoidanceModule::updateState()
   DebugData debug;
   const auto avoid_data = calcAvoidancePlanningData(debug);
   const bool has_avoidance_target = !avoid_data.objects.empty();
-
   if (!is_plan_running && !has_avoidance_target) {
     current_state_ = BT::NodeStatus::SUCCESS;
   } else {
@@ -191,41 +195,51 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
   debug_linestring.clear();
   // for filtered objects
   ObjectDataArray target_objects;
+  std::vector<AvoidanceDebugMsg> avoidance_debug_msg_array;
   for (const auto & i : lane_filtered_objects_index) {
     const auto & object = objects_candidate.objects.at(i);
     const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position;
+    AvoidanceDebugMsg avoidance_debug_msg;
+    const auto avoidance_debug_array_false_and_push_back =
+      [&avoidance_debug_msg, &avoidance_debug_msg_array](const std::string & failed_reason) {
+        avoidance_debug_msg.allow_avoidance = false;
+        avoidance_debug_msg.failed_reason = failed_reason;
+        avoidance_debug_msg_array.push_back(avoidance_debug_msg);
+      };
 
     if (!isTargetObjectType(object)) {
-      DEBUG_PRINT("Ignore object: (isTargetObjectType is false)");
+      avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE);
       continue;
     }
 
     ObjectData object_data;
     object_data.object = object;
-
+    avoidance_debug_msg.object_id = getUuidStr(object_data);
     // calc longitudinal distance from ego to closest target object footprint point.
     object_data.longitudinal = calcDistanceToClosestFootprintPoint(reference_path, object, ego_pos);
+    avoidance_debug_msg.longitudinal_distance = object_data.longitudinal;
 
     // object is behind ego or too far.
     if (object_data.longitudinal < -parameters_.object_check_backward_distance) {
-      DEBUG_PRINT("Ignore object: (object < -backward_distance threshold)");
+      avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD);
       continue;
     }
     if (object_data.longitudinal > parameters_.object_check_forward_distance) {
-      DEBUG_PRINT("Ignore object: (object > forward_distance threshold)");
+      avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD);
       continue;
     }
 
     // Target object is behind the path goal -> ignore.
     if (object_data.longitudinal > dist_to_goal) {
-      DEBUG_PRINT("Ignore object: (object is behind the path goal)");
+      avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL);
       continue;
     }
 
     // Calc lateral deviation from path to target object.
     const auto object_closest_index = findNearestIndex(path_points, object_pos);
     const auto object_closest_pose = path_points.at(object_closest_index).point.pose;
     object_data.lateral = calcLateralDeviation(object_closest_pose, object_pos);
+    avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral;
 
     // Find the footprint point closest to the path, set to object_data.overhang_distance.
     object_data.overhang_dist =
@@ -283,8 +297,9 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
       object_data.to_road_shoulder_distance);
 
     // Object is on center line -> ignore.
+    avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral;
     if (std::abs(object_data.lateral) < parameters_.threshold_distance_object_is_on_center) {
-      DEBUG_PRINT("Ignore object: (object is on center line)");
+      avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE);
       continue;
     }
 
@@ -294,6 +309,21 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
 
   // debug
   {
+    debug_data_.avoidance_debug_msg_array.avoidance_info.clear();
+    auto & debug_data_avoidance = debug_data_.avoidance_debug_msg_array.avoidance_info;
+    debug_data_avoidance = avoidance_debug_msg_array;
+    if (!debug_avoidance_initializer_for_shift_point_.empty()) {
+      const bool is_info_old_ =
+        (clock_->now() - debug_avoidance_initializer_for_shift_point_time_).seconds() > 1.0;
+      if (!is_info_old_) {
+        debug_data_avoidance.insert(
+          debug_data_avoidance.end(), debug_avoidance_initializer_for_shift_point_.begin(),
+          debug_avoidance_initializer_for_shift_point_.end());
+      }
+    }
+
+    debug_avoidance_msg_array_ptr_ =
+      std::make_shared<AvoidanceDebugMsgArray>(debug_data_.avoidance_debug_msg_array);
     debug.farthest_linestring_from_overhang =
       std::make_shared<lanelet::ConstLineStrings3d>(debug_linestring);
     debug.current_lanelets = std::make_shared<lanelet::ConstLanelets>(current_lanes);
@@ -449,6 +479,7 @@ void AvoidanceModule::registerRawShiftPoints(const AvoidPointArray & future)
 AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
   const ObjectDataArray & objects) const
 {
+  debug_avoidance_initializer_for_shift_point_.clear();
   const auto prepare_distance = getNominalPrepareDistance();
 
   // To be consistent with changes in the ego position, the current shift length is considered.
@@ -465,7 +496,28 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
     lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width;
 
   AvoidPointArray avoid_points;
+  std::vector<AvoidanceDebugMsg> avoidance_debug_msg_array;
+  avoidance_debug_msg_array.reserve(objects.size());
   for (auto & o : objects) {
+    AvoidanceDebugMsg avoidance_debug_msg;
+    const auto avoidance_debug_array_false_and_push_back =
+      [&avoidance_debug_msg, &avoidance_debug_msg_array](const std::string & failed_reason) {
+        avoidance_debug_msg.allow_avoidance = false;
+        avoidance_debug_msg.failed_reason = failed_reason;
+        avoidance_debug_msg_array.push_back(avoidance_debug_msg);
+      };
+
+    avoidance_debug_msg.object_id = getUuidStr(o);
+    avoidance_debug_msg.longitudinal_distance = o.longitudinal;
+    avoidance_debug_msg.lateral_distance_from_centerline = o.lateral;
+    avoidance_debug_msg.to_furthest_linestring_distance = o.to_road_shoulder_distance;
+    avoidance_debug_msg.max_shift_length = max_allowable_lateral_distance;
+
+    if (!(o.to_road_shoulder_distance > max_allowable_lateral_distance)) {
+      avoidance_debug_msg.allow_avoidance = false;
+      avoidance_debug_msg.failed_reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN;
+    }
+
     const auto max_shift_length =
       o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width;
     const auto max_left_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length,
@@ -507,17 +559,18 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
         // TODO(Horibe) Even if there is no enough distance for avoidance shift, the
         // return-to-center shift must be considered for each object if the current_shift
         // is not zero.
-        DEBUG_PRINT("object is ignored since remaining_distance <= 0");
+        avoidance_debug_array_false_and_push_back(
+          AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO);
         continue;
       }
 
       // This is the case of exceeding the jerk limit. Use the sharp avoidance ego speed.
       const auto required_jerk = path_shifter_.calcJerkFromLatLonDistance(
         avoiding_shift, remaining_distance, getSharpAvoidanceEgoSpeed());
+      avoidance_debug_msg.required_jerk = required_jerk;
+      avoidance_debug_msg.maximum_jerk = parameters_.max_lateral_jerk;
       if (required_jerk > parameters_.max_lateral_jerk) {
-        DEBUG_PRINT(
-          "object is ignored required_jerk is too large (req: %f, max: %f)", required_jerk,
-          parameters_.max_lateral_jerk);
+        avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::TOO_LARGE_JERK);
         continue;
       }
     }
@@ -563,8 +616,12 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
       avoiding_shift, return_shift, ap_avoid.start_longitudinal, ap_avoid.end_longitudinal,
       ap_return.end_longitudinal, nominal_avoid_distance, avoiding_distance, avoid_margin,
       nominal_return_distance);
+    avoidance_debug_msg.allow_avoidance = true;
+    avoidance_debug_msg_array.push_back(avoidance_debug_msg);
   }
 
+  debug_avoidance_initializer_for_shift_point_ = std::move(avoidance_debug_msg_array);
+  debug_avoidance_initializer_for_shift_point_time_ = clock_->now();
   fillAdditionalInfoFromLongitudinal(avoid_points);
 
   return avoid_points;
@@ -2403,22 +2460,6 @@ void AvoidanceModule::updateData()
   }
 }
 
-std::string getUuidStr(const ObjectData & obj)
-{
-  return std::to_string(obj.object.object_id.uuid.at(0)) +
-         std::to_string(obj.object.object_id.uuid.at(1)) +
-         std::to_string(obj.object.object_id.uuid.at(2));
-}
-
-std::string getUuidStr(const ObjectDataArray & objs)
-{
-  std::stringstream ss;
-  for (const auto & o : objs) {
-    ss << getUuidStr(o) << ", ";
-  }
-  return ss.str();
-}
-
 /*
  * updateRegisteredObject
  *
@@ -2540,6 +2581,7 @@ void AvoidanceModule::initVariables()
   prev_reference_ = PathWithLaneId();
   path_shifter_ = PathShifter{};
 
+  debug_avoidance_msg_array_ptr_.reset();
   debug_data_ = DebugData();
 
   registered_raw_shift_points_ = {};