Merge branch 'tier4/main' into ci/dispatch-push-event-main

common/tier4_traffic_light_rviz_plugin/README.md

@@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals.
 
 ### Output
 
-| Name                                                    | Type                                                     | Description                   |
-| ------------------------------------------------------- | -------------------------------------------------------- | ----------------------------- |
-| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals |
+| Name                                                    | Type                                                | Description                   |
+| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- |
+| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals |
 
 ## HowToUse
 

common/tier4_traffic_light_rviz_plugin/package.xml

@@ -11,7 +11,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_auto_mapping_msgs</depend>
-  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>

common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp

@@ -34,6 +34,7 @@
 #include <utility>
 #include <vector>
 
+#undef signals
 namespace rviz_plugins
 {
 TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_common::Panel(parent)
@@ -138,55 +139,55 @@ void TrafficLightPublishPanel::onSetTrafficLightState()
   const auto shape = light_shape_combo_->currentText();
   const auto status = light_status_combo_->currentText();
 
-  TrafficLight traffic_light;
+  TrafficSignalElement traffic_light;
   traffic_light.confidence = traffic_light_confidence_input_->value();
 
   if (color == "RED") {
-    traffic_light.color = TrafficLight::RED;
+    traffic_light.color = TrafficSignalElement::RED;
   } else if (color == "AMBER") {
-    traffic_light.color = TrafficLight::AMBER;
+    traffic_light.color = TrafficSignalElement::AMBER;
   } else if (color == "GREEN") {
-    traffic_light.color = TrafficLight::GREEN;
+    traffic_light.color = TrafficSignalElement::GREEN;
   } else if (color == "WHITE") {
-    traffic_light.color = TrafficLight::WHITE;
+    traffic_light.color = TrafficSignalElement::WHITE;
   } else if (color == "UNKNOWN") {
-    traffic_light.color = TrafficLight::UNKNOWN;
+    traffic_light.color = TrafficSignalElement::UNKNOWN;
   }
 
   if (shape == "CIRCLE") {
-    traffic_light.shape = TrafficLight::CIRCLE;
+    traffic_light.shape = TrafficSignalElement::CIRCLE;
   } else if (shape == "LEFT_ARROW") {
-    traffic_light.shape = TrafficLight::LEFT_ARROW;
+    traffic_light.shape = TrafficSignalElement::LEFT_ARROW;
   } else if (shape == "RIGHT_ARROW") {
-    traffic_light.shape = TrafficLight::RIGHT_ARROW;
+    traffic_light.shape = TrafficSignalElement::RIGHT_ARROW;
   } else if (shape == "UP_ARROW") {
-    traffic_light.shape = TrafficLight::UP_ARROW;
+    traffic_light.shape = TrafficSignalElement::UP_ARROW;
   } else if (shape == "DOWN_ARROW") {
-    traffic_light.shape = TrafficLight::DOWN_ARROW;
+    traffic_light.shape = TrafficSignalElement::DOWN_ARROW;
   } else if (shape == "DOWN_LEFT_ARROW") {
-    traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW;
+    traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW;
   } else if (shape == "DOWN_RIGHT_ARROW") {
-    traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW;
+    traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW;
   } else if (shape == "UNKNOWN") {
-    traffic_light.shape = TrafficLight::UNKNOWN;
+    traffic_light.shape = TrafficSignalElement::UNKNOWN;
   }
 
   if (status == "SOLID_OFF") {
-    traffic_light.status = TrafficLight::SOLID_OFF;
+    traffic_light.status = TrafficSignalElement::SOLID_OFF;
   } else if (status == "SOLID_ON") {
-    traffic_light.status = TrafficLight::SOLID_ON;
+    traffic_light.status = TrafficSignalElement::SOLID_ON;
   } else if (status == "FLASHING") {
-    traffic_light.status = TrafficLight::FLASHING;
+    traffic_light.status = TrafficSignalElement::FLASHING;
   } else if (status == "UNKNOWN") {
-    traffic_light.status = TrafficLight::UNKNOWN;
+    traffic_light.status = TrafficSignalElement::UNKNOWN;
   }
 
   TrafficSignal traffic_signal;
-  traffic_signal.lights.push_back(traffic_light);
-  traffic_signal.map_primitive_id = traffic_light_id;
+  traffic_signal.elements.push_back(traffic_light);
+  traffic_signal.traffic_signal_id = traffic_light_id;
 
   for (auto & signal : extra_traffic_signals_.signals) {
-    if (signal.map_primitive_id == traffic_light_id) {
+    if (signal.traffic_signal_id == traffic_light_id) {
       signal = traffic_signal;
       return;
     }
@@ -247,7 +248,7 @@ void TrafficLightPublishPanel::createWallTimer()
 void TrafficLightPublishPanel::onTimer()
 {
   if (enable_publish_) {
-    extra_traffic_signals_.header.stamp = rclcpp::Clock().now();
+    extra_traffic_signals_.stamp = rclcpp::Clock().now();
     pub_traffic_signals_->publish(extra_traffic_signals_);
   }
 
@@ -260,35 +261,35 @@ void TrafficLightPublishPanel::onTimer()
   for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) {
     const auto & signal = extra_traffic_signals_.signals.at(i);
 
-    if (signal.lights.empty()) {
+    if (signal.elements.empty()) {
       continue;
     }
 
-    auto id_label = new QLabel(QString::number(signal.map_primitive_id));
+    auto id_label = new QLabel(QString::number(signal.traffic_signal_id));
     id_label->setAlignment(Qt::AlignCenter);
 
     auto color_label = new QLabel();
     color_label->setAlignment(Qt::AlignCenter);
 
-    const auto & light = signal.lights.front();
+    const auto & light = signal.elements.front();
     switch (light.color) {
-      case TrafficLight::RED:
+      case TrafficSignalElement::RED:
         color_label->setText("RED");
         color_label->setStyleSheet("background-color: #FF0000;");
         break;
-      case TrafficLight::AMBER:
+      case TrafficSignalElement::AMBER:
         color_label->setText("AMBER");
         color_label->setStyleSheet("background-color: #FFBF00;");
         break;
-      case TrafficLight::GREEN:
+      case TrafficSignalElement::GREEN:
         color_label->setText("GREEN");
         color_label->setStyleSheet("background-color: #7CFC00;");
         break;
-      case TrafficLight::WHITE:
+      case TrafficSignalElement::WHITE:
         color_label->setText("WHITE");
         color_label->setStyleSheet("background-color: #FFFFFF;");
         break;
-      case TrafficLight::UNKNOWN:
+      case TrafficSignalElement::UNKNOWN:
         color_label->setText("UNKNOWN");
         color_label->setStyleSheet("background-color: #808080;");
         break;
@@ -300,31 +301,28 @@ void TrafficLightPublishPanel::onTimer()
     shape_label->setAlignment(Qt::AlignCenter);
 
     switch (light.shape) {
-      case TrafficLight::CIRCLE:
+      case TrafficSignalElement::CIRCLE:
         shape_label->setText("CIRCLE");
         break;
-      case TrafficLight::LEFT_ARROW:
+      case TrafficSignalElement::LEFT_ARROW:
         shape_label->setText("LEFT_ARROW");
         break;
-      case TrafficLight::RIGHT_ARROW:
+      case TrafficSignalElement::RIGHT_ARROW:
         shape_label->setText("RIGHT_ARROW");
         break;
-      case TrafficLight::UP_ARROW:
+      case TrafficSignalElement::UP_ARROW:
         shape_label->setText("UP_ARROW");
         break;
-      case TrafficLight::DOWN_ARROW:
+      case TrafficSignalElement::DOWN_ARROW:
         shape_label->setText("DOWN_ARROW");
         break;
-      case TrafficLight::DOWN_LEFT_ARROW:
+      case TrafficSignalElement::DOWN_LEFT_ARROW:
         shape_label->setText("DOWN_LEFT_ARROW");
         break;
-      case TrafficLight::DOWN_RIGHT_ARROW:
+      case TrafficSignalElement::DOWN_RIGHT_ARROW:
         shape_label->setText("DOWN_RIGHT_ARROW");
         break;
-      case TrafficLight::FLASHING:
-        shape_label->setText("FLASHING");
-        break;
-      case TrafficLight::UNKNOWN:
+      case TrafficSignalElement::UNKNOWN:
         shape_label->setText("UNKNOWN");
         break;
       default:
@@ -335,16 +333,16 @@ void TrafficLightPublishPanel::onTimer()
     status_label->setAlignment(Qt::AlignCenter);
 
     switch (light.status) {
-      case TrafficLight::SOLID_OFF:
+      case TrafficSignalElement::SOLID_OFF:
         status_label->setText("SOLID_OFF");
         break;
-      case TrafficLight::SOLID_ON:
+      case TrafficSignalElement::SOLID_ON:
         status_label->setText("SOLID_ON");
         break;
-      case TrafficLight::FLASHING:
+      case TrafficSignalElement::FLASHING:
         status_label->setText("FLASHING");
         break;
-      case TrafficLight::UNKNOWN:
+      case TrafficSignalElement::UNKNOWN:
         status_label->setText("UNKNOWN");
         break;
       default:
@@ -375,11 +373,9 @@ void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg)
   std::string info = "Fetching traffic lights :";
   std::string delim = " ";
   for (auto && tl_reg_elem_ptr : tl_reg_elems) {
-    for (auto && traffic_light : tl_reg_elem_ptr->trafficLights()) {
-      auto id = static_cast<int>(traffic_light.id());
-      info += (std::exchange(delim, ", ") + std::to_string(id));
-      traffic_light_ids_.insert(id);
-    }
+    auto id = static_cast<int>(tl_reg_elem_ptr->id());
+    info += (std::exchange(delim, ", ") + std::to_string(id));
+    traffic_light_ids_.insert(id);
   }
   RCLCPP_INFO_STREAM(raw_node_->get_logger(), info);
   received_vector_map_ = true;

common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp

@@ -27,7 +27,7 @@
 #include <rviz_common/panel.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
-#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #endif
 
 #include <set>
@@ -36,10 +36,9 @@ namespace rviz_plugins
 {
 
 using autoware_auto_mapping_msgs::msg::HADMapBin;
-using autoware_auto_perception_msgs::msg::TrafficLight;
-using autoware_auto_perception_msgs::msg::TrafficSignal;
-using autoware_auto_perception_msgs::msg::TrafficSignalArray;
-
+using autoware_perception_msgs::msg::TrafficSignal;
+using autoware_perception_msgs::msg::TrafficSignalArray;
+using autoware_perception_msgs::msg::TrafficSignalElement;
 class TrafficLightPublishPanel : public rviz_common::Panel
 {
   Q_OBJECT

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -177,10 +177,6 @@ def launch_setup(context, *args, **kwargs):
                 "~/input/traffic_signals",
                 "/perception/traffic_light_recognition/traffic_signals",
             ),
-            (
-                "~/input/external_traffic_signals",
-                "/external/traffic_light_recognition/traffic_signals",
-            ),
             (
                 "~/input/external_velocity_limit_mps",
                 "/planning/scenario_planning/max_velocity_default",

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp

@@ -46,7 +46,9 @@ using visualization_msgs::msg::MarkerArray;
 
 // TODO(sugahara) move to util
 PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
-lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side);
+lanelet::ConstLanelets getPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance);
 PredictedObjects filterObjectsByLateralDistance(
   const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
   const double distance_thresh, const bool filter_inside);

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

@@ -881,17 +881,35 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
     }
 
     // output avoidance path under lateral jerk constraints.
-    const auto feasible_shift_length = PathShifter::calcLateralDistFromJerk(
+    const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk(
       remaining_distance, helper_.getLateralMaxJerkLimit(), helper_.getAvoidanceEgoSpeed());
 
-    RCLCPP_WARN_THROTTLE(
-      getLogger(), *clock_, 1000,
-      "original shift length is not feasible. generate avoidance path under the constraints. "
-      "[original: (%.2f) actual: (%.2f)]",
-      std::abs(avoiding_shift), feasible_shift_length);
+    if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) {
+      object.reason = "LessThanExecutionThreshold";
+      return boost::none;
+    }
+
+    const auto feasible_shift_length =
+      desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift
+                                : -1.0 * feasible_relative_shift_length + current_ego_shift;
+
+    const auto feasible =
+      std::abs(feasible_shift_length - object.overhang_dist) <
+      0.5 * planner_data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
+    if (feasible) {
+      RCLCPP_WARN_THROTTLE(
+        getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. ");
+      object.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN;
+      return boost::none;
+    }
+
+    {
+      RCLCPP_WARN_THROTTLE(
+        getLogger(), *clock_, 1000, "use feasible shift length. [original: (%.2f) actual: (%.2f)]",
+        std::abs(avoiding_shift), feasible_relative_shift_length);
+    }
 
-    return desire_shift_length > 0.0 ? feasible_shift_length + current_ego_shift
-                                     : -1.0 * feasible_shift_length + current_ego_shift;
+    return feasible_shift_length;
   };
 
   const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -335,8 +335,9 @@ bool GoalPlannerModule::isExecutionRequested() const
   // if (A) or (B) is met execute pull over
   // (A) target lane is `road` and same to the current lanes
   // (B) target lane is `road_shoulder` and neighboring to the current lanes
-  const lanelet::ConstLanelets pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_);
+  const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(route_handler), left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
   lanelet::ConstLanelet target_lane{};
   lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane);
   if (!isCrossingPossible(current_lane, target_lane)) {
@@ -387,8 +388,9 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const
   const double base_link2front = planner_data_->parameters.base_link2front;
   const double base_link2rear = planner_data_->parameters.base_link2rear;
 
-  const lanelet::ConstLanelets pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_);
+  const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
 
   lanelet::Lanelet closest_pull_over_lanelet{};
   lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet);
@@ -643,8 +645,9 @@ void GoalPlannerModule::setLanes()
     planner_data_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length,
     /*forward_only_in_route*/ false);
-  status_.pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_);
+  status_.pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
   status_.lanes =
     utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes);
 }
@@ -1487,8 +1490,9 @@ bool GoalPlannerModule::isSafePath() const
   const auto current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
-  const auto pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_);
+  const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *route_handler, left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
   const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points);
   const auto & common_param = planner_data_->parameters;
   const std::pair<double, double> terminal_velocity_and_accel =
@@ -1655,8 +1659,9 @@ bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const
   const auto & route_handler = planner_data_->route_handler;
   const Pose & goal_pose = route_handler->getOriginalGoalPose();
 
-  const lanelet::ConstLanelets pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_);
+  const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(route_handler), left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
   lanelet::ConstLanelet target_lane{};
   lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane);
 

planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp

@@ -63,8 +63,9 @@ boost::optional<PullOverPath> FreespacePullOver::plan(const Pose & goal_pose)
   const auto road_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length,
     /*forward_only_in_route*/ false);
-  const auto pull_over_lanes =
-    goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_);
+  const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
+    parameters_.forward_goal_search_length);
   if (road_lanes.empty() || pull_over_lanes.empty()) {
     return {};
   }