Merge branch 'main' into RT1-8920-revise-current-lane-objects-filtering

Author: mkquda

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp

@@ -18,6 +18,7 @@
 #include "autoware/behavior_path_lane_change_module/structs/debug.hpp"
 #include "autoware/behavior_path_lane_change_module/structs/path.hpp"
 #include "autoware/behavior_path_lane_change_module/utils/utils.hpp"
+#include "autoware/behavior_path_planner_common/data_manager.hpp"
 #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp"
 #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
@@ -38,6 +39,7 @@
 class TestNormalLaneChange;
 namespace autoware::behavior_path_planner
 {
+using autoware::behavior_path_planner::PoseWithDetailOpt;
 using autoware::route_handler::Direction;
 using autoware::universe_utils::StopWatch;
 using geometry_msgs::msg::Point;
@@ -222,7 +224,7 @@ class LaneChangeBase
     return direction_;
   }
 
-  std::optional<Pose> getStopPose() const { return lane_change_stop_pose_; }
+  PoseWithDetailOpt getStopPose() const { return lane_change_stop_pose_; }
 
   void resetStopPose() { lane_change_stop_pose_ = std::nullopt; }
 
@@ -282,7 +284,7 @@ class LaneChangeBase
   lane_change::CommonDataPtr common_data_ptr_;
   FilteredLanesObjects filtered_objects_{};
   BehaviorModuleOutput prev_module_output_{};
-  std::optional<Pose> lane_change_stop_pose_{std::nullopt};
+  PoseWithDetailOpt lane_change_stop_pose_{std::nullopt};
 
   PathWithLaneId prev_approved_path_;
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

@@ -18,6 +18,7 @@
 #include "autoware/behavior_path_lane_change_module/structs/data.hpp"
 
 #include <memory>
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -157,7 +158,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool check_prepare_phase() const;
 
-  void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path);
+  void set_stop_pose(
+    const double arc_length_to_stop_pose, PathWithLaneId & path, const std::string & reason = "");
 
   void updateStopTime();
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp

@@ -45,7 +45,7 @@ struct Debug
   double distance_to_end_of_current_lane{std::numeric_limits<double>::max()};
   double distance_to_lane_change_finished{std::numeric_limits<double>::max()};
   double distance_to_abort_finished{std::numeric_limits<double>::max()};
-  bool is_able_to_return_to_current_lane{false};
+  bool is_able_to_return_to_current_lane{true};
   bool is_stuck{false};
   bool is_abort{false};
 
@@ -69,7 +69,7 @@ struct Debug
     distance_to_end_of_current_lane = std::numeric_limits<double>::max();
     distance_to_lane_change_finished = std::numeric_limits<double>::max();
     distance_to_abort_finished = std::numeric_limits<double>::max();
-    is_able_to_return_to_current_lane = false;
+    is_able_to_return_to_current_lane = true;
     is_stuck = false;
     is_abort = false;
   }

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml

@@ -26,6 +26,7 @@
   <depend>autoware_motion_utils</depend>
   <depend>autoware_rtc_interface</depend>
   <depend>autoware_universe_utils</depend>
+  <depend>fmt</depend>
   <depend>pluginlib</depend>
   <depend>range-v3</depend>
   <depend>rclcpp</depend>

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

@@ -111,9 +111,7 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   path_reference_ = std::make_shared<PathWithLaneId>(output.reference_path);
   *prev_approved_path_ = getPreviousModuleOutput().path;
 
-  const auto stop_pose_opt = module_type_->getStopPose();
-  stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value()))
-                                         : PoseWithDetailOpt();
+  stop_pose_ = module_type_->getStopPose();
 
   const auto & lane_change_debug = module_type_->getDebugData();
   for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) {
@@ -171,9 +169,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   }
 
   path_reference_ = std::make_shared<PathWithLaneId>(out.reference_path);
-  const auto stop_pose_opt = module_type_->getStopPose();
-  stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value()))
-                                         : PoseWithDetailOpt();
+  stop_pose_ = module_type_->getStopPose();
 
   if (!module_type_->isValidPath()) {
     path_candidate_ = std::make_shared<PathWithLaneId>();

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -408,7 +408,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
         output.path.points, output.path.points.front().point.pose.position, getEgoPosition());
       const auto stop_dist =
         -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
-      set_stop_pose(stop_dist + current_dist, output.path);
+      set_stop_pose(stop_dist + current_dist, output.path, "incoming rear object");
     } else {
       insert_stop_point(get_target_lanes(), output.path);
     }
@@ -471,7 +471,7 @@ void NormalLaneChange::insert_stop_point(
   if (!is_current_lane) {
     const auto arc_length_to_stop_pose = motion_utils::calcArcLength(path.points) -
                                          common_data_ptr_->transient_data.next_dist_buffer.min;
-    set_stop_pose(arc_length_to_stop_pose, path);
+    set_stop_pose(arc_length_to_stop_pose, path, "next lc terminal");
     return;
   }
 
@@ -514,10 +514,11 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path)
 
   const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width);
 
+  const auto terminal_stop_reason = status_.is_valid_path ? "no safe path" : "no valid path";
   if (
     filtered_objects_.current_lane.empty() ||
     !lane_change_parameters_->enable_stopped_vehicle_buffer) {
-    set_stop_pose(dist_to_terminal_stop, path);
+    set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason);
     return;
   }
 
@@ -543,12 +544,12 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path)
   const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin;
 
   if (stop_arc_length_behind_obj < dist_to_target_lane_start) {
-    set_stop_pose(dist_to_target_lane_start, path);
+    set_stop_pose(dist_to_target_lane_start, path, "maintain distance to front object");
     return;
   }
 
   if (stop_arc_length_behind_obj > dist_to_terminal_stop) {
-    set_stop_pose(dist_to_terminal_stop, path);
+    set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason);
     return;
   }
 
@@ -564,11 +565,11 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path)
     filtered_objects_.target_lane_leading, stop_arc_length_behind_obj, path);
 
   if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) {
-    set_stop_pose(dist_to_terminal_stop, path);
+    set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason);
     return;
   }
 
-  set_stop_pose(stop_arc_length_behind_obj, path);
+  set_stop_pose(stop_arc_length_behind_obj, path, "maintain distance to front object");
 }
 
 PathWithLaneId NormalLaneChange::getReferencePath() const
@@ -1684,10 +1685,11 @@ bool NormalLaneChange::is_ego_stuck() const
   return has_object_blocking;
 }
 
-void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path)
+void NormalLaneChange::set_stop_pose(
+  const double arc_length_to_stop_pose, PathWithLaneId & path, const std::string & reason)
 {
   const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path);
-  lane_change_stop_pose_ = stop_point.point.pose;
+  lane_change_stop_pose_ = PoseWithDetailOpt(PoseWithDetail(stop_point.point.pose, reason));
 }
 
 void NormalLaneChange::updateStopTime()

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp

@@ -25,10 +25,11 @@
 #include <visualization_msgs/msg/detail/marker__struct.hpp>
 #include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 
+#include <fmt/format.h>
+
 #include <algorithm>
 #include <cstdint>
 #include <cstdlib>
-#include <sstream>
 #include <string>
 #include <vector>
 
@@ -52,27 +53,6 @@ MarkerArray showAllValidLaneChangePath(
   const auto loop_size = std::min(lane_change_paths.size(), colors.size());
   marker_array.markers.reserve(loop_size);
 
-  const auto info_prep_to_string =
-    [](const autoware::behavior_path_planner::lane_change::Info & info) -> std::string {
-    std::ostringstream ss_text;
-    ss_text << std::setprecision(3) << "vel: " << info.velocity.prepare
-            << "[m/s] | lon_acc: " << info.longitudinal_acceleration.prepare
-            << "[m/s2] | t: " << info.duration.prepare << "[s] | L: " << info.length.prepare
-            << "[m]";
-    return ss_text.str();
-  };
-
-  const auto info_lc_to_string =
-    [](const autoware::behavior_path_planner::lane_change::Info & info) -> std::string {
-    std::ostringstream ss_text;
-    ss_text << std::setprecision(3) << "vel: " << info.velocity.lane_changing
-            << "[m/s] | lon_acc: " << info.longitudinal_acceleration.lane_changing
-            << "[m/s2] | lat_acc: " << info.lateral_acceleration
-            << "[m/s2] | t: " << info.duration.lane_changing
-            << "[s] | L: " << info.length.lane_changing << "[m]";
-    return ss_text.str();
-  };
-
   for (std::size_t idx = 0; idx < loop_size; ++idx) {
     int32_t id{0};
     const auto & lc_path = lane_change_paths.at(idx);
@@ -91,19 +71,30 @@ MarkerArray showAllValidLaneChangePath(
       marker.points.push_back(point.point.pose.position);
     }
 
+    const auto & info = lc_path.info;
     auto text_marker = createDefaultMarker(
       "map", current_time, ns_with_idx, ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
       createMarkerScale(0.1, 0.1, 0.8), colors::yellow());
     const auto prep_idx = points.size() / 4;
     text_marker.pose = points.at(prep_idx).point.pose;
     text_marker.pose.position.z += 2.0;
-    text_marker.text = info_prep_to_string(lc_path.info);
+    text_marker.text = fmt::format(
+      "vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | t: {time:.3f}[s] | L: {length:.3f}[m]",
+      fmt::arg("vel", info.velocity.prepare),
+      fmt::arg("lon_acc", info.longitudinal_acceleration.prepare),
+      fmt::arg("time", info.duration.prepare), fmt::arg("length", info.length.prepare));
     marker_array.markers.push_back(text_marker);
 
     const auto lc_idx = points.size() / 2;
     text_marker.id = ++id;
     text_marker.pose = points.at(lc_idx).point.pose;
-    text_marker.text = info_lc_to_string(lc_path.info);
+    text_marker.text = fmt::format(
+      "vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | lat_acc: {lat_acc:.3f}[m/s2] | t: "
+      "{time:.3f}[s] | L: {length:.3f}[m]",
+      fmt::arg("vel", info.velocity.lane_changing),
+      fmt::arg("lon_acc", info.longitudinal_acceleration.lane_changing),
+      fmt::arg("lat_acc", info.lateral_acceleration), fmt::arg("time", info.duration.lane_changing),
+      fmt::arg("length", info.length.lane_changing));
     marker_array.markers.push_back(text_marker);
 
     marker_array.markers.push_back(marker);
@@ -186,17 +177,10 @@ MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg
   safety_check_info_text.pose = ego_pose;
   safety_check_info_text.pose.position.z += 4.0;
 
-  std::ostringstream ss;
-
-  ss << "\nDistToEndOfCurrentLane: " << std::setprecision(5)
-     << debug_data.distance_to_end_of_current_lane
-     << "\nDistToLaneChangeFinished: " << debug_data.distance_to_lane_change_finished
-     << (debug_data.is_stuck ? "\nVehicleStuck" : "")
-     << (debug_data.is_able_to_return_to_current_lane ? "\nAbleToReturnToCurrentLane" : "")
-     << (debug_data.is_abort ? "\nAborting" : "")
-     << "\nDistanceToAbortFinished: " << debug_data.distance_to_abort_finished;
-
-  safety_check_info_text.text = ss.str();
+  safety_check_info_text.text = fmt::format(
+    "{stuck} | {return_lane} | {abort}", fmt::arg("stuck", debug_data.is_stuck ? "is stuck" : ""),
+    fmt::arg("return_lane", debug_data.is_able_to_return_to_current_lane ? "" : "can't return"),
+    fmt::arg("abort", debug_data.is_abort ? "aborting" : ""));
   marker_array.markers.push_back(safety_check_info_text);
   return marker_array;
 }

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml

@@ -23,6 +23,7 @@
 
   <depend>autoware_behavior_velocity_planner_common</depend>
   <depend>autoware_grid_map_utils</depend>
+  <depend>autoware_internal_debug_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
@@ -41,7 +42,6 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
-  <depend>tier4_debug_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py

@@ -19,10 +19,10 @@
 from copy import deepcopy
 
 from ament_index_python.packages import get_package_share_directory
+from autoware_internal_debug_msgs.msg import StringStamped
 import matplotlib.pyplot as plt
 import rclpy
 from rclpy.node import Node
-from tier4_debug_msgs.msg import StringStamped
 import yaml
 
 

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -153,11 +153,11 @@ StopFactor createStopFactor(
   return stop_factor;
 }
 
-tier4_debug_msgs::msg::StringStamped createStringStampedMessage(
+autoware_internal_debug_msgs::msg::StringStamped createStringStampedMessage(
   const rclcpp::Time & now, const int64_t module_id_,
   const std::vector<std::tuple<std::string, CollisionPoint, CollisionState>> & collision_points)
 {
-  tier4_debug_msgs::msg::StringStamped msg;
+  autoware_internal_debug_msgs::msg::StringStamped msg;
   msg.stamp = now;
   for (const auto & collision_point : collision_points) {
     std::stringstream ss;
@@ -199,8 +199,8 @@ CrosswalkModule::CrosswalkModule(
 
   road_ = lanelet_map_ptr->laneletLayer.get(lane_id);
 
-  collision_info_pub_ =
-    node.create_publisher<tier4_debug_msgs::msg::StringStamped>("~/debug/collision_info", 1);
+  collision_info_pub_ = node.create_publisher<autoware_internal_debug_msgs::msg::StringStamped>(
+    "~/debug/collision_info", 1);
 }
 
 bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path)

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -23,9 +23,9 @@
 #include <autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_internal_debug_msgs/msg/string_stamped.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <tier4_debug_msgs/msg/string_stamped.hpp>
 
 #include <boost/assert.hpp>
 #include <boost/assign/list_of.hpp>
@@ -456,7 +456,8 @@ class CrosswalkModule : public SceneModuleInterface
 
   const int64_t module_id_;
 
-  rclcpp::Publisher<tier4_debug_msgs::msg::StringStamped>::SharedPtr collision_info_pub_;
+  rclcpp::Publisher<autoware_internal_debug_msgs::msg::StringStamped>::SharedPtr
+    collision_info_pub_;
 
   lanelet::ConstLanelet crosswalk_;
 

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml

@@ -20,6 +20,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_behavior_velocity_planner_common</depend>
+  <depend>autoware_internal_debug_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py

@@ -22,13 +22,13 @@
 import time
 
 from PIL import Image
+from autoware_internal_debug_msgs.msg import Float64MultiArrayStamped
 import imageio
 import matplotlib
 import matplotlib.pyplot as plt
 import numpy as np
 import rclpy
 from rclpy.node import Node
-from tier4_debug_msgs.msg import Float64MultiArrayStamped
 
 matplotlib.use("TKAgg")
 

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -88,10 +88,11 @@ IntersectionModule::IntersectionModule(
     static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP);
   }
 
-  ego_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
+  ego_ttc_pub_ = node.create_publisher<autoware_internal_debug_msgs::msg::Float64MultiArrayStamped>(
     "~/debug/intersection/ego_ttc", 1);
-  object_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
-    "~/debug/intersection/object_ttc", 1);
+  object_ttc_pub_ =
+    node.create_publisher<autoware_internal_debug_msgs::msg::Float64MultiArrayStamped>(
+      "~/debug/intersection/object_ttc", 1);
 }
 
 bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path)
@@ -231,7 +232,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat
   // calculate the expected vehicle speed and obtain the spatiotemporal profile of ego to the
   // exit of intersection
   // ==========================================================================================
-  tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array;
+  autoware_internal_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array;
   const auto time_distance_array =
     calcIntersectionPassingTime(*path, is_prioritized, intersection_stoplines, &ego_ttc_time_array);
 
@@ -240,7 +241,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat
   // passed each pass judge line for the first time, save current collision status for late
   // diagnosis
   // ==========================================================================================
-  tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array;
+  autoware_internal_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array;
   updateObjectInfoManagerCollision(
     path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line,
     safely_passed_2nd_judge_line, &object_ttc_time_array);