Merge branch 'main' into refactor/use_run_base_class_function

Signed-off-by: kyoichi-sugahara <[email protected]>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -5,6 +5,112 @@
   <arg name="input_vector_map_topic_name" default="/map/vector_map"/>
   <arg name="input_pointcloud_map_topic_name" default="/map/pointcloud_map"/>
 
+  <arg name="launch_avoidance_module" default="true"/>
+  <arg name="launch_avoidance_by_lane_change_module" default="true"/>
+  <arg name="launch_dynamic_avoidance_module" default="true"/>
+  <arg name="launch_lane_change_right_module" default="true"/>
+  <arg name="launch_lane_change_left_module" default="true"/>
+  <arg name="launch_external_request_lane_change_right_module" default="true"/>
+  <arg name="launch_external_request_lane_change_left_module" default="true"/>
+  <arg name="launch_goal_planner_module" default="true"/>
+  <arg name="launch_start_planner_module" default="true"/>
+  <arg name="launch_side_shift_module" default="true"/>
+
+  <arg name="launch_crosswalk_module" default="true"/>
+  <arg name="launch_walkway_module" default="true"/>
+  <arg name="launch_traffic_light_module" default="true"/>
+  <arg name="launch_intersection_module" default="true"/>
+  <arg name="launch_merge_from_private_module" default="true"/>
+  <arg name="launch_blind_spot_module" default="true"/>
+  <arg name="launch_detection_area_module" default="true"/>
+  <arg name="launch_virtual_traffic_light_module" default="true"/>
+  <arg name="launch_no_stopping_area_module" default="true"/>
+  <arg name="launch_stop_line_module" default="true"/>
+  <arg name="launch_occlusion_spot_module" default="true"/>
+  <arg name="launch_run_out_module" default="true"/>
+  <arg name="launch_speed_bump_module" default="true"/>
+  <arg name="launch_out_of_lane_module" default="true"/>
+  <arg name="launch_no_drivable_lane_module" default="true"/>
+
+  <!-- assemble launch config for behavior velocity planner -->
+  <arg name="behavior_velocity_planner_launch_modules" default="["/>
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::CrosswalkModulePlugin, '&quot;)"
+    if="$(var launch_crosswalk_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::WalkwayModulePlugin, '&quot;)"
+    if="$(var launch_walkway_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::TrafficLightModulePlugin, '&quot;)"
+    if="$(var launch_traffic_light_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::IntersectionModulePlugin, '&quot;)"
+    if="$(var launch_intersection_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::MergeFromPrivateModulePlugin, '&quot;)"
+    if="$(var launch_merge_from_private_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::BlindSpotModulePlugin, '&quot;)"
+    if="$(var launch_blind_spot_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::DetectionAreaModulePlugin, '&quot;)"
+    if="$(var launch_detection_area_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::VirtualTrafficLightModulePlugin, '&quot;)"
+    if="$(var launch_virtual_traffic_light_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::NoStoppingAreaModulePlugin, '&quot;)"
+    if="$(var launch_no_stopping_area_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::StopLineModulePlugin, '&quot;)"
+    if="$(var launch_stop_line_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::OcclusionSpotModulePlugin, '&quot;)"
+    if="$(var launch_occlusion_spot_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::RunOutModulePlugin, '&quot;)"
+    if="$(var launch_run_out_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::SpeedBumpModulePlugin, '&quot;)"
+    if="$(var launch_speed_bump_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::OutOfLaneModulePlugin, '&quot;)"
+    if="$(var launch_out_of_lane_module)"
+  />
+  <let
+    name="behavior_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::NoDrivableLaneModulePlugin, '&quot;)"
+    if="$(var launch_no_drivable_lane_module)"
+  />
+  <let name="behavior_velocity_planner_launch_modules" value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + ']'&quot;)"/>
+
   <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="screen">
     <composable_node pkg="behavior_path_planner" plugin="behavior_path_planner::BehaviorPathPlannerNode" name="behavior_path_planner" namespace="">
       <!-- topic remap -->
@@ -23,6 +129,16 @@
       <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
       <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
       <!-- params -->
+      <param name="avoidance.enable_module" value="$(var launch_avoidance_module)"/>
+      <param name="avoidance_by_lc.enable_module" value="$(var launch_avoidance_by_lane_change_module)"/>
+      <param name="dynamic_avoidance.enable_module" value="$(var launch_dynamic_avoidance_module)"/>
+      <param name="lane_change_right.enable_module" value="$(var launch_lane_change_right_module)"/>
+      <param name="lane_change_left.enable_module" value="$(var launch_lane_change_left_module)"/>
+      <param name="external_request_lane_change_left.enable_module" value="$(var launch_external_request_lane_change_left_module)"/>
+      <param name="external_request_lane_change_right.enable_module" value="$(var launch_external_request_lane_change_right_module)"/>
+      <param name="goal_planner.enable_module" value="$(var launch_goal_planner_module)"/>
+      <param name="start_planner.enable_module" value="$(var launch_start_planner_module)"/>
+      <param name="side_shift.enable_module" value="$(var launch_side_shift_module)"/>
       <param from="$(var common_param_path)"/>
       <param from="$(var vehicle_param_file)"/>
       <param from="$(var nearest_search_param_path)"/>
@@ -66,6 +182,7 @@
       <param from="$(var motion_velocity_smoother_param_path)"/>
       <param from="$(var behavior_velocity_smoother_type_param_path)"/>
       <param from="$(var behavior_velocity_planner_common_param_path)"/>
+      <param name="launch_modules" value="$(var behavior_velocity_planner_launch_modules)"/>
       <!-- <param from="$(var template_param_path)"/> -->
       <param from="$(var behavior_velocity_planner_blind_spot_module_param_path)"/>
       <param from="$(var behavior_velocity_planner_crosswalk_module_param_path)"/>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

@@ -180,7 +180,7 @@
   </group>
 
   <!-- surround obstacle check -->
-  <group if="$(var enable_surround_check)">
+  <group if="$(var launch_surround_obstacle_checker)">
     <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
       <composable_node pkg="surround_obstacle_checker" plugin="surround_obstacle_checker::SurroundObstacleCheckerNode" name="surround_obstacle_checker" namespace="">
         <!-- topic remap -->

perception/lidar_apollo_segmentation_tvm/README.md

@@ -6,9 +6,10 @@
 
 #### Neural network
 
-This package will not build without a neural network for its inference.
-The network is provided by the cmake function exported by the tvm_utility package.
-See its design page for more information on how to enable downloading pre-compiled networks (by setting the `DOWNLOAD_ARTIFACTS` cmake variable), or how to handle user-compiled networks.
+This package will not run without a neural network for its inference.
+The network is provided by ansible script during the installation of Autoware or can be downloaded manually according to [Manual Downloading](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts).
+This package uses 'get_neural_network' function from tvm_utility package to create and provide proper dependency.
+See its design page for more information on how to handle user-compiled networks.
 
 #### Backend
 

perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp

@@ -0,0 +1,55 @@
+// Copyright 2021 Arm Limited and Contributors.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "tvm_utility/pipeline.hpp"
+
+#ifndef PERCEPTION__LIDAR_APOLLO_SEGMENTATION_TVM__DATA__MODELS__BAIDU_CNN__INFERENCE_ENGINE_TVM_CONFIG_HPP_  // NOLINT
+#define PERCEPTION__LIDAR_APOLLO_SEGMENTATION_TVM__DATA__MODELS__BAIDU_CNN__INFERENCE_ENGINE_TVM_CONFIG_HPP_  // NOLINT
+
+namespace model_zoo
+{
+namespace perception
+{
+namespace lidar_obstacle_detection
+{
+namespace baidu_cnn
+{
+namespace onnx_bcnn
+{
+
+static const tvm_utility::pipeline::InferenceEngineTVMConfig config{
+  {3, 0, 0},  // modelzoo_version
+
+  "baidu_cnn",  // network_name
+  "llvm",       // network_backend
+
+  "./deploy_lib.so",        // network_module_path
+  "./deploy_graph.json",    // network_graph_path
+  "./deploy_param.params",  // network_params_path
+
+  kDLCPU,  // tvm_device_type
+  0,       // tvm_device_id
+
+  {{"data", kDLFloat, 32, 1, {1, 4, 864, 864}}},  // network_inputs
+
+  {{"deconv0", kDLFloat, 32, 1, {1, 12, 864, 864}}}  // network_outputs
+};
+
+}  // namespace onnx_bcnn
+}  // namespace baidu_cnn
+}  // namespace lidar_obstacle_detection
+}  // namespace perception
+}  // namespace model_zoo
+// NOLINTNEXTLINE
+#endif  // PERCEPTION__LIDAR_APOLLO_SEGMENTATION_TVM__DATA__MODELS__BAIDU_CNN__INFERENCE_ENGINE_TVM_CONFIG_HPP_

perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp

@@ -141,10 +141,11 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC ApolloLidarSegmentation
   /// \param[in] height_thresh If it is non-negative, the points that are higher than the predicted
   ///                          object height by height_thresh are filtered out in the
   ///                          post-processing step.
+  /// \param[in] data_path The path to autoware data and artifacts folder
   explicit ApolloLidarSegmentation(
     int32_t range, float score_threshold, bool use_intensity_feature, bool use_constant_feature,
     float z_offset, float min_height, float max_height, float objectness_thresh,
-    int32_t min_pts_num, float height_thresh);
+    int32_t min_pts_num, float height_thresh, const std::string & data_path);
 
   /// \brief Detect obstacles.
   /// \param[in] input Input pointcloud.

perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp

@@ -108,7 +108,7 @@ std::shared_ptr<DetectedObjectsWithFeature> ApolloLidarSegmentationPostProcessor
 ApolloLidarSegmentation::ApolloLidarSegmentation(
   int32_t range, float score_threshold, bool use_intensity_feature, bool use_constant_feature,
   float z_offset, float min_height, float max_height, float objectness_thresh, int32_t min_pts_num,
-  float height_thresh)
+  float height_thresh, const std::string & data_path)
 : range_(range),
   score_threshold_(score_threshold),
   z_offset_(z_offset),
@@ -118,7 +118,7 @@ ApolloLidarSegmentation::ApolloLidarSegmentation(
   pcl_pointcloud_ptr_(new pcl::PointCloud<pcl::PointXYZI>),
   PreP(std::make_shared<PrePT>(
     config, range, use_intensity_feature, use_constant_feature, min_height, max_height)),
-  IE(std::make_shared<IET>(config, "lidar_apollo_segmentation_tvm")),
+  IE(std::make_shared<IET>(config, "lidar_apollo_segmentation_tvm", data_path)),
   PostP(std::make_shared<PostPT>(
     config, pcl_pointcloud_ptr_, range, objectness_thresh, score_threshold, height_thresh,
     min_pts_num)),

perception/lidar_apollo_segmentation_tvm/test/main.cpp

@@ -17,14 +17,18 @@
 
 #include <gtest/gtest.h>
 
+#include <filesystem>
 #include <memory>
 #include <random>
 #include <string>
 #include <vector>
 
 using autoware::perception::lidar_apollo_segmentation_tvm::ApolloLidarSegmentation;
+namespace fs = std::filesystem;
 
-void test_segmentation(bool use_intensity_feature, bool use_constant_feature, bool expect_throw)
+void test_segmentation(
+  const std::string & data_path, bool use_intensity_feature, bool use_constant_feature,
+  bool expect_throw)
 {
   // Instantiate the pipeline
   const int width = 1;
@@ -37,9 +41,10 @@ void test_segmentation(bool use_intensity_feature, bool use_constant_feature, bo
   const float objectness_thresh = 0.5f;
   const int32_t min_pts_num = 3;
   const float height_thresh = 0.5f;
+
   ApolloLidarSegmentation segmentation(
     range, score_threshold, use_intensity_feature, use_constant_feature, z_offset, min_height,
-    max_height, objectness_thresh, min_pts_num, height_thresh);
+    max_height, objectness_thresh, min_pts_num, height_thresh, data_path);
 
   auto version_status = segmentation.version_check();
   EXPECT_NE(version_status, tvm_utility::Version::Unsupported);
@@ -85,8 +90,31 @@ void test_segmentation(bool use_intensity_feature, bool use_constant_feature, bo
 // Other test configurations to increase code coverage.
 TEST(lidar_apollo_segmentation_tvm, others)
 {
-  test_segmentation(false, true, false);
-  test_segmentation(true, true, false);
-  test_segmentation(false, false, false);
-  test_segmentation(true, false, false);
+  std::string home = std::getenv("HOME");
+  fs::path data_path(home);
+  data_path /= "autoware_data";
+  fs::path apollo_data_path(data_path);
+  apollo_data_path /= "lidar_apollo_segmentation_tvm";
+  fs::path deploy_path(apollo_data_path);
+  deploy_path /= "models/baidu_cnn";
+
+  fs::path deploy_graph("deploy_graph.json");
+  fs::path deploy_lib("deploy_lib.so");
+  fs::path deploy_param("deploy_param.params");
+
+  fs::path deploy_graph_path = deploy_path / deploy_graph;
+  fs::path deploy_lib_path = deploy_path / deploy_lib;
+  fs::path deploy_param_path = deploy_path / deploy_param;
+
+  if (
+    !fs::exists(deploy_graph_path) || !fs::exists(deploy_lib_path) ||
+    !fs::exists(deploy_param_path)) {
+    printf("Model deploy files not found. Skip test.\n");
+    GTEST_SKIP();
+    return;
+  }
+  test_segmentation(data_path, false, true, false);
+  test_segmentation(data_path, true, true, false);
+  test_segmentation(data_path, false, false, false);
+  test_segmentation(data_path, true, false, false);
 }

perception/lidar_apollo_segmentation_tvm_nodes/README.md

@@ -28,7 +28,7 @@ See the design of the algorithm in the core (lidar_apollo_segmentation_tvm) pack
 
 ### Usage
 
-`lidar_apollo_segmentation_tvm` and `lidar_apollo_segmentation_tvm_nodes` will not build without a neural network.
+`lidar_apollo_segmentation_tvm` and `lidar_apollo_segmentation_tvm_nodes` will not work without a neural network.
 See the lidar_apollo_segmentation_tvm usage for more information.
 
 ### Assumptions / Known limits

perception/lidar_apollo_segmentation_tvm_nodes/config/lidar_apollo_segmentation_tvm_nodes.param.yaml

@@ -24,3 +24,4 @@
     objectness_thresh: 0.5
     min_pts_num: 3
     height_thresh: 0.5
+    data_path: $(env HOME)/autoware_data

perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py

@@ -21,20 +21,25 @@
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-import yaml
+import launch_ros.parameter_descriptions
 
 
 def generate_launch_description():
     param_file = os.path.join(
         get_package_share_directory("lidar_apollo_segmentation_tvm_nodes"),
         "config/lidar_apollo_segmentation_tvm_nodes.param.yaml",
     )
-    with open(param_file, "r") as f:
-        lidar_apollo_segmentation_tvm_node_params = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+    lidar_apollo_segmentation_tvm_node_params = launch_ros.parameter_descriptions.ParameterFile(
+        param_file=param_file, allow_substs=True
+    )
 
     arguments = [
         DeclareLaunchArgument("input/pointcloud", default_value="/sensing/lidar/pointcloud"),
         DeclareLaunchArgument("output/objects", default_value="labeled_clusters"),
+        DeclareLaunchArgument(
+            "data_path", default_value=os.path.join(os.environ["HOME"], "autoware_data")
+        ),
     ]
 
     # lidar segmentation node execution definition.

perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml

@@ -23,6 +23,7 @@
     <remap from="points_in" to="$(var input/pointcloud)"/>
     <remap from="objects_out" to="$(var output/objects)"/>
     <param name="z_offset" value="$(var z_offset)"/>
-    <param from="$(var param_file)"/>
+    <!-- cspell: ignore substs -->
+    <param from="$(var param_file)" allow_substs="true"/>
   </node>
 </launch>

perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json

@@ -61,6 +61,11 @@
           "type": "number",
           "default": 0.5,
           "description": "If it is non-negative, the points that are higher than the predicted object height by height_thresh are filtered out in the post-processing step."
+        },
+        "data_path": {
+          "type": "string",
+          "default": "$(env HOME)/autoware_data",
+          "description": "Packages data and artifacts directory path."
         }
       },
       "required": [
@@ -73,7 +78,8 @@
         "max_height",
         "objectness_thresh",
         "min_pts_num",
-        "height_thresh"
+        "height_thresh",
+        "data_path"
       ]
     }
   },