Merge pull request autowarefoundation#362 from tier4/sync-upstream

chore: sync upstream

.github/CODEOWNERS

@@ -27,6 +27,7 @@ common/polar_grid/** [email protected] @autowarefoundation/autoware-global
 common/rtc_manager_rviz_plugin/** [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 common/signal_processing/** [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 common/tensorrt_common/** [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
+common/tier4_adapi_rviz_plugin/** [email protected] [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 common/tier4_api_utils/** [email protected] [email protected] [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 common/tier4_automatic_goal_rviz_plugin/** [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 common/tier4_autoware_utils/** [email protected] [email protected] [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
@@ -133,7 +134,7 @@ planning/obstacle_cruise_planner/** [email protected] yutaka.shimizu@tie
 planning/obstacle_stop_planner/** [email protected] [email protected] [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 planning/obstacle_velocity_limiter/** [email protected] @autowarefoundation/autoware-global-codeowners
 planning/planning_debug_tools/** [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
-planning/planning_test_utils/** [email protected] @autowarefoundation/autoware-global-codeowners
+planning/planning_test_utils/** [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 planning/planning_validator/** [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 planning/route_handler/** [email protected] [email protected] [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 planning/rtc_auto_mode_manager/** [email protected] [email protected] @autowarefoundation/autoware-global-codeowners

perception/traffic_light_classifier/README.md

@@ -93,7 +93,7 @@ These colors and shapes are assigned to the message as follows:
 
 ## Customization of CNN model
 
-Currently, in Autoware, [MobileNetV2](https://arxiv.org/abs/1801.04381v3) is used as CNN classifier by default. The corresponding onnx file is [data/traffic_light_classifier_mobilenetv2.onnx](./data/traffic_light_classifier_mobilenetv2.onnx).
+Currently, in Autoware, [MobileNetV2](https://arxiv.org/abs/1801.04381v3) is used as CNN classifier by default. The corresponding onnx file is `data/traffic_light_classifier_mobilenetv2.onnx`(This file will be downloaded during the build process).
 Also, you can apply the following models shown as below, for example.
 
 - [EfficientNet](https://arxiv.org/abs/1905.11946v5)
@@ -145,7 +145,7 @@ $ mim train mmcls YOUR_CONFIG.py [--resume-from YOUR_CHECKPOINT.pth]
 
 ### step 3. Export onnx model
 
-In exporting onnx, use `mmclassificatoin/tools/deployment/pytorch2onnx.py` or [open-mmlab/mmdeploy](https://github.com/open-mmlab/mmdeploy.git).
+In exporting onnx, use `mmclassification/tools/deployment/pytorch2onnx.py` or [open-mmlab/mmdeploy](https://github.com/open-mmlab/mmdeploy.git).
 
 ```shell
 cd ~/mmclassification/tools/deployment

perception/traffic_light_classifier/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>The traffic_light_classifier package</description>
   <maintainer email="[email protected]">Yukihiro Saito</maintainer>
+  <maintainer email="[email protected]">Shunsuke Miura</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

planning/behavior_path_planner/CMakeLists.txt

@@ -25,7 +25,7 @@ set(common_src
   src/utils/path_utils.cpp
   src/utils/safety_check.cpp
   src/utils/avoidance/util.cpp
-  src/utils/lane_change/util.cpp
+  src/utils/lane_change/utils.cpp
   src/utils/side_shift/util.cpp
   src/utils/pull_over/util.cpp
   src/utils/pull_over/shift_pull_over.cpp
@@ -114,6 +114,12 @@ if(BUILD_TESTING)
     behavior_path_planner_node
   )
 
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node_interface
+    test/test_${PROJECT_NAME}_node_interface.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}_node_interface
+    behavior_path_planner_node
+  )
 endif()
 
 ament_auto_package(

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -6,16 +6,17 @@
       resample_interval_for_output: 4.0                 # [m]
       detection_area_right_expand_dist: 0.0             # [m]
       detection_area_left_expand_dist: 1.0              # [m]
-      object_envelope_buffer: 0.3                       # [m]
+      drivable_area_right_bound_offset: 0.0             # [m]
+      drivable_area_left_bound_offset: 0.0              # [m]
 
       # avoidance module common setting
       enable_bound_clipping: false
       enable_avoidance_over_same_direction: true
       enable_avoidance_over_opposite_direction: true
       enable_update_path_when_object_is_gone: false
       enable_force_avoidance_for_stopped_vehicle: false
-      enable_safety_check: false
-      enable_yield_maneuver: false
+      enable_safety_check: true
+      enable_yield_maneuver: true
       disable_path_update: false
 
       # for debug
@@ -24,14 +25,46 @@
 
       # avoidance is performed for the object type with true
       target_object:
-        car: true
-        truck: true
-        bus: true
-        trailer: true
-        unknown: false
-        bicycle: false
-        motorcycle: false
-        pedestrian: false
+        car:
+          enable: true
+          envelope_buffer_margin: 0.3
+          safety_buffer_lateral: 0.7
+          safety_buffer_longitudinal: 0.0
+        truck:
+          enable: true
+          envelope_buffer_margin: 0.3
+          safety_buffer_lateral: 0.7
+          safety_buffer_longitudinal: 0.0
+        bus:
+          enable: true
+          envelope_buffer_margin: 0.3
+          safety_buffer_lateral: 0.7
+          safety_buffer_longitudinal: 0.0
+        trailer:
+          enable: true
+          envelope_buffer_margin: 0.3
+          safety_buffer_lateral: 0.7
+          safety_buffer_longitudinal: 0.0
+        unknown:
+          enable: false
+          envelope_buffer_margin: 0.3
+          safety_buffer_lateral: 0.7
+          safety_buffer_longitudinal: 0.0
+        bicycle:
+          enable: false
+          envelope_buffer_margin: 0.8
+          safety_buffer_lateral: 1.0
+          safety_buffer_longitudinal: 1.0
+        motorcycle:
+          enable: false
+          envelope_buffer_margin: 0.8
+          safety_buffer_lateral: 1.0
+          safety_buffer_longitudinal: 1.0
+        pedestrian:
+          enable: false
+          envelope_buffer_margin: 0.8
+          safety_buffer_lateral: 1.0
+          safety_buffer_longitudinal: 1.0
 
       # For target object filtering
       target_filtering:
@@ -64,16 +97,14 @@
         # avoidance lateral parameters
         lateral:
           lateral_collision_margin: 1.0                 # [m]
-          lateral_collision_safety_buffer: 0.7          # [m]
-          lateral_passable_safety_buffer: 0.0           # [m]
+          lateral_passable_safety_buffer: 1.0           # [m]
           road_shoulder_safety_margin: 0.3              # [m]
           avoidance_execution_lateral_threshold: 0.499
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0
         # avoidance distance parameters
         longitudinal:
           prepare_time: 2.0                             # [s]
-          longitudinal_collision_safety_buffer: 0.0     # [m]
           min_prepare_distance: 1.0                     # [m]
           min_avoidance_distance: 10.0                  # [m]
           min_nominal_avoidance_speed: 7.0              # [m/s]

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -22,10 +22,9 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <diagnostic_msgs/msg/diagnostic_status.hpp>
-#include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <algorithm>
+#include <limits>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -184,6 +183,32 @@ class PlannerManager
     return result;
   }
 
+  /**
+   * @brief get reference path from root_lanelet_ centerline.
+   * @param planner data.
+   * @return reference path.
+   */
+  BehaviorModuleOutput getReferencePath(const std::shared_ptr<PlannerData> & data) const
+  {
+    const auto & route_handler = data->route_handler;
+    const auto & pose = data->self_odometry->pose.pose;
+    const auto p = data->parameters;
+
+    constexpr double extra_margin = 10.0;
+    const auto backward_length =
+      std::max(p.backward_path_length, p.backward_path_length + extra_margin);
+
+    const auto lanelet_sequence = route_handler->getLaneletSequence(
+      root_lanelet_.get(), pose, backward_length, std::numeric_limits<double>::max());
+
+    lanelet::ConstLanelet closest_lane{};
+    if (!lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) {
+      return {};
+    }
+
+    return util::getReferencePath(closest_lane, parameters_, data);
+  }
+
   /**
    * @brief stop and unregister the module from manager.
    * @param module.
@@ -278,13 +303,6 @@ class PlannerManager
    */
   BehaviorModuleOutput runApprovedModules(const std::shared_ptr<PlannerData> & data);
 
-  /**
-   * @brief get reference path from root_lanelet_ centerline.
-   * @param planner data.
-   * @return reference path.
-   */
-  BehaviorModuleOutput getReferencePath(const std::shared_ptr<PlannerData> & data) const;
-
   /**
    * @brief select a module that should be execute at first.
    * @param modules that make execution request.

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp

@@ -54,7 +54,7 @@ class LaneFollowingModule : public SceneModuleInterface
 private:
   std::shared_ptr<LaneFollowingParameters> parameters_;
 
-  PathWithLaneId getReferencePath() const;
+  BehaviorModuleOutput getReferencePath() const;
   void initParam();
 };
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/util.hpp → planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTIL_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTIL_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_
 
 #include "behavior_path_planner/marker_util/lane_change/debug.hpp"
 #include "behavior_path_planner/parameters.hpp"
@@ -36,7 +36,7 @@
 #include <utility>
 #include <vector>
 
-namespace behavior_path_planner::util::lane_change
+namespace behavior_path_planner::utils::lane_change
 {
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
@@ -181,5 +181,5 @@ lanelet::ConstLanelets getLaneChangeLanes(
   const std::shared_ptr<const PlannerData> & planner_data,
   const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length,
   const double prepare_duration, const Direction direction, const LaneChangeModuleType type);
-}  // namespace behavior_path_planner::util::lane_change
-#endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTIL_HPP_
+}  // namespace behavior_path_planner::utils::lane_change
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/marker_util/debug_utilities.hpp"
 #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/utils/lane_following/module_data.hpp"
 #include "behavior_path_planner/utils/pull_out/pull_out_path.hpp"
 #include "motion_utils/motion_utils.hpp"
 #include "perception_utils/predicted_path_utils.hpp"
@@ -309,6 +310,11 @@ PathWithLaneId setDecelerationVelocity(
   const PathWithLaneId & input, const double target_velocity, const Pose target_pose,
   const double buffer, const double deceleration_interval);
 
+BehaviorModuleOutput getReferencePath(
+  const lanelet::ConstLanelet & current_lane,
+  const std::shared_ptr<LaneFollowingParameters> & parameters,
+  const std::shared_ptr<const PlannerData> & planner_data);
+
 // object label
 std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
 

planning/behavior_path_planner/package.xml

@@ -58,6 +58,7 @@
   <depend>magic_enum</depend>
   <depend>motion_utils</depend>
   <depend>perception_utils</depend>
+  <depend>planning_test_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>route_handler</depend>

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1125,14 +1125,20 @@ void BehaviorPathPlannerNode::run()
   // NOTE: In order to keep backward_path_length at least, resampling interval is added to the
   // backward.
   const auto current_pose = planner_data_->self_odometry->pose.pose;
-  const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(path->points);
-  path->points = motion_utils::cropPoints(
-    path->points, current_pose.position, current_seg_idx,
-    planner_data_->parameters.forward_path_length,
-    planner_data_->parameters.backward_path_length + planner_data_->parameters.input_path_interval);
-
   if (!path->points.empty()) {
-    path_publisher_->publish(*path);
+    const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(path->points);
+    path->points = motion_utils::cropPoints(
+      path->points, current_pose.position, current_seg_idx,
+      planner_data_->parameters.forward_path_length,
+      planner_data_->parameters.backward_path_length +
+        planner_data_->parameters.input_path_interval);
+
+    if (!path->points.empty()) {
+      path_publisher_->publish(*path);
+    } else {
+      RCLCPP_ERROR_THROTTLE(
+        get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish.");
+    }
   } else {
     RCLCPP_ERROR_THROTTLE(
       get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish.");
@@ -1473,6 +1479,11 @@ void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg)
 }
 void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg)
 {
+  if (msg->segments.empty()) {
+    RCLCPP_ERROR(get_logger(), "input route is empty. ignored");
+    return;
+  }
+
   const std::lock_guard<std::mutex> lock(mutex_route_);
   route_ptr_ = msg;
   has_received_route_ = true;

planning/behavior_path_planner/src/planner_manager.cpp

@@ -450,70 +450,6 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
   return approved_modules_output;
 }
 
-BehaviorModuleOutput PlannerManager::getReferencePath(
-  const std::shared_ptr<PlannerData> & data) const
-{
-  PathWithLaneId reference_path{};
-
-  constexpr double extra_margin = 10.0;
-
-  const auto & route_handler = data->route_handler;
-  const auto & pose = data->self_odometry->pose.pose;
-  const auto p = data->parameters;
-
-  reference_path.header = route_handler->getRouteHeader();
-
-  const auto backward_length =
-    std::max(p.backward_path_length, p.backward_path_length + extra_margin);
-
-  const auto lanelet_sequence = route_handler->getLaneletSequence(
-    root_lanelet_.get(), pose, backward_length, std::numeric_limits<double>::max());
-
-  lanelet::ConstLanelet closest_lane{};
-  if (!lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) {
-    return {};
-  }
-
-  const auto current_lanes =
-    route_handler->getLaneletSequence(closest_lane, pose, backward_length, p.forward_path_length);
-
-  reference_path = util::getCenterLinePath(
-    *route_handler, current_lanes, pose, backward_length, p.forward_path_length, p);
-
-  // clip backward length
-  const size_t current_seg_idx = data->findEgoSegmentIndex(reference_path.points);
-  util::clipPathLength(
-    reference_path, current_seg_idx, p.forward_path_length, p.backward_path_length);
-  const auto drivable_lanelets = util::getLaneletsFromPath(reference_path, route_handler);
-  const auto drivable_lanes = util::generateDrivableLanes(drivable_lanelets);
-
-  {
-    const int num_lane_change =
-      std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back()));
-
-    const double lane_change_buffer = util::calcLaneChangeBuffer(p, num_lane_change);
-
-    reference_path = util::setDecelerationVelocity(
-      *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration,
-      lane_change_buffer);
-  }
-
-  const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes);
-
-  const auto expanded_lanes = util::expandLanelets(
-    shorten_lanes, parameters_->drivable_area_left_bound_offset,
-    parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip);
-
-  util::generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, data);
-
-  BehaviorModuleOutput output;
-  output.path = std::make_shared<PathWithLaneId>(reference_path);
-  output.reference_path = std::make_shared<PathWithLaneId>(reference_path);
-  output.drivable_lanes = drivable_lanes;
-
-  return output;
-}
-
 void PlannerManager::updateCandidateModules(
   const std::vector<SceneModulePtr> & request_modules,
   const SceneModulePtr & highest_priority_module)