feat: add lidar apollo instance segmentation

perception/lidar_apollo_instance_segmentation/.gitignore

@@ -0,0 +1,2 @@
+data/*.caffemodel
+data/*.engine

perception/lidar_apollo_instance_segmentation/CMakeLists.txt

@@ -0,0 +1,175 @@
+cmake_minimum_required(VERSION 3.5)
+project(lidar_apollo_instance_segmentation)
+
+option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF)
+
+# set flags for CUDA availability
+option(CUDA_AVAIL "CUDA available" OFF)
+find_package(CUDA)
+if (CUDA_FOUND)
+  find_library(CUBLAS_LIBRARIES cublas HINTS
+    ${CUDA_TOOLKIT_ROOT_DIR}/lib64
+    ${CUDA_TOOLKIT_ROOT_DIR}/lib
+  )
+  if (CUDA_VERBOSE)
+    message("CUDA is available!")
+    message("CUDA Libs: ${CUDA_LIBRARIES}")
+    message("CUDA Headers: ${CUDA_INCLUDE_DIRS}")
+  endif ()
+  set(CUDA_AVAIL ON)
+else()
+  message("CUDA NOT FOUND")
+  set(CUDA_AVAIL OFF)
+endif (CUDA_FOUND)
+
+# set flags for TensorRT availability
+option(TRT_AVAIL "TensorRT available" OFF)
+# try to find the tensorRT modules
+find_library(NVINFER NAMES nvinfer)
+find_library(NVPARSERS NAMES nvparsers)
+find_library(NVCAFFE_PARSER NAMES nvcaffe_parser)
+find_library(NVINFER_PLUGIN NAMES nvinfer_plugin)
+if(NVINFER AND NVPARSERS AND NVCAFFE_PARSER AND NVINFER_PLUGIN)
+  if (CUDA_VERBOSE)
+    message("TensorRT is available!")
+    message("NVINFER: ${NVINFER}")
+    message("NVPARSERS: ${NVPARSERS}")
+    message("NVCAFFE_PARSER: ${NVCAFFE_PARSER}")
+  endif ()
+  set(TRT_AVAIL ON)
+else()
+  message("TensorRT is NOT Available")
+  set(TRT_AVAIL OFF)
+endif()
+
+# set flags for CUDNN availability
+option(CUDNN_AVAIL "CUDNN available" OFF)
+# try to find the CUDNN module
+find_library(CUDNN_LIBRARY
+NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name}
+PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX}
+PATH_SUFFIXES lib lib64 bin
+DOC "CUDNN library." )
+if(CUDNN_LIBRARY)
+  if (CUDA_VERBOSE)
+    message("CUDNN is available!")
+    message("CUDNN_LIBRARY: ${CUDNN_LIBRARY}")
+  endif ()
+  set(CUDNN_AVAIL ON)
+else()
+  message("CUDNN is NOT Available")
+  set(CUDNN_AVAIL OFF)
+endif()
+
+find_program(GDOWN_AVAIL "gdown")
+if (NOT GDOWN_AVAIL)
+  message("gdown: command not found. External files could not be downloaded.")
+endif()
+if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
+  # download weight files
+  set(PATH "${CMAKE_CURRENT_SOURCE_DIR}/data")
+  if (NOT EXISTS "${PATH}")
+    execute_process(COMMAND mkdir -p ${PATH})
+  endif()
+  set(FILE "${PATH}/vlp-16.caffemodel")
+  message(STATUS "Checking and downloading vlp-16.caffemodel")
+  if (NOT EXISTS "${FILE}")
+    message(STATUS "... file does not exist. Downloading now ...")
+    execute_process(COMMAND gdown "https://drive.google.com/uc?id=1bbO_eeGG5HUqyUiAYjOd6hUn-Zma0mMJ" -O ${PATH}/vlp-16.caffemodel)
+  endif()
+  set(FILE "${PATH}/hdl-64.caffemodel")
+  message(STATUS "Checking and downloading hdl-64.caffemodel")
+  if (NOT EXISTS "${FILE}")
+    message(STATUS "... file does not exist. Downloading now ...")
+    execute_process(COMMAND gdown "https://drive.google.com/uc?id=1ZdB6V3jua3GmtYuY9NR1nc9QZe_ChjkP" -O ${PATH}/hdl-64.caffemodel)
+  endif()
+  set(FILE "${PATH}/vls-128.caffemodel")
+  message(STATUS "Checking and downloading vls-128.caffemodel")
+  if (NOT EXISTS "${FILE}")
+    message(STATUS "... file does not exist. Downloading now ...")
+    execute_process(COMMAND gdown "https://drive.google.com/uc?id=1izpNotNxS6mrYIzJwHQ_EyX_IPDU-MBr" -O ${PATH}/vls-128.caffemodel)
+  endif()
+
+  find_package(ament_cmake_auto REQUIRED)
+  ament_auto_find_build_dependencies()
+  find_package(PCL REQUIRED)
+
+  if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 14)
+    set(CMAKE_CXX_STANDARD_REQUIRED ON)
+    set(CMAKE_CXX_EXTENSIONS OFF)
+  endif()
+  if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Werror)
+  endif()
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_MWAITXINTRIN_H_INCLUDED")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_FORCE_INLINES")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D__STRICT_ANSI__")
+
+  include_directories(
+    lib/include
+    ${CUDA_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+  )
+
+  ament_auto_add_library(tensorrt_apollo_cnn_lib SHARED
+    lib/src/TrtNet.cpp
+  )
+
+  target_link_libraries(tensorrt_apollo_cnn_lib
+    ${NVINFER}
+    ${NVCAFFE_PARSER}
+    ${NVINFER_PLUGIN}
+    ${CUDA_LIBRARIES}
+    ${CUBLAS_LIBRARIES}
+    ${CUDA_curand_LIBRARY}
+    ${CUDNN_LIBRARY}
+    ${PCL_LIBRARIES}
+  )
+
+  ament_auto_add_library(lidar_apollo_instance_segmentation SHARED
+    src/node.cpp
+    src/detector.cpp
+    src/log_table.cpp
+    src/feature_generator.cpp
+    src/feature_map.cpp
+    src/cluster2d.cpp
+    src/debugger.cpp
+  )
+
+  target_link_libraries(lidar_apollo_instance_segmentation
+    tensorrt_apollo_cnn_lib
+  )
+
+  rclcpp_components_register_node(lidar_apollo_instance_segmentation
+    PLUGIN "LidarInstanceSegmentationNode"
+    EXECUTABLE lidar_apollo_instance_segmentation_node
+  )
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+  ament_auto_package(INSTALL_TO_SHARE
+    launch
+    config
+    data
+  )
+
+else()
+  find_package(ament_cmake_auto REQUIRED)
+  ament_auto_find_build_dependencies()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+  # to avoid launch file missing without a gpu
+  ament_auto_package(INSTALL_TO_SHARE
+    launch
+  )
+
+endif()

perception/lidar_apollo_instance_segmentation/README.md

@@ -0,0 +1,143 @@
+# lidar_apollo_instance_segmentation
+
+![Peek 2020-04-07 00-17](https://user-images.githubusercontent.com/8327598/78574862-92507d80-7865-11ea-9a2d-56d3453bdb7a.gif)
+
+## Purpose
+
+This node segments 3D pointcloud data from lidar sensors into obstacles, e.g., cars, trucks, bicycles, and pedestrians
+based on CNN based model and obstacle clustering method.
+
+## Inner-workings / Algorithms
+
+See the [original design](https://github.com/ApolloAuto/apollo/blob/master/docs/specs/3d_obstacle_perception.md) by Apollo.
+
+## Inputs / Outputs
+
+### Input
+
+| Name               | Type                      | Description                        |
+| ------------------ | ------------------------- | ---------------------------------- |
+| `input/pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud data from lidar sensors |
+
+### Output
+
+| Name                        | Type                                                  | Description                                       |
+| --------------------------- | ----------------------------------------------------- | ------------------------------------------------- |
+| `output/labeled_clusters`   | `autoware_perception_msgs/DetectedObjectsWithFeature` | Detected objects with labeled pointcloud cluster. |
+| `debug/instance_pointcloud` | `sensor_msgs/PointCloud2`                             | Segmented pointcloud for visualization.           |
+
+## Parameters
+
+### Node Parameters
+
+None
+
+### Core Parameters
+
+| Name                    | Type   | Default Value        | Description                                                                        |
+| ----------------------- | ------ | -------------------- | ---------------------------------------------------------------------------------- |
+| `score_threshold`       | double | 0.8                  | If the score of a detected object is lower than this value, the object is ignored. |
+| `range`                 | int    | 60                   | Half of the length of feature map sides. [m]                                       |
+| `width`                 | int    | 640                  | The grid width of feature map.                                                     |
+| `height`                | int    | 640                  | The grid height of feature map.                                                    |
+| `engine_file`           | string | "vls-128.engine"     | The name of TensorRT engine file for CNN model.                                    |
+| `prototxt_file`         | string | "vls-128.prototxt"   | The name of prototxt file for CNN model.                                           |
+| `caffemodel_file`       | string | "vls-128.caffemodel" | The name of caffemodel file for CNN model.                                         |
+| `use_intensity_feature` | bool   | true                 | The flag to use intensity feature of pointcloud.                                   |
+| `use_constant_feature`  | bool   | false                | The flag to use direction and distance feature of pointcloud.                      |
+| `target_frame`          | string | "base_link"          | Pointcloud data is transformed into this frame.                                    |
+| `z_offset`              | int    | 2                    | z offset from target frame. [m]                                                    |
+
+## Assumptions / Known limits
+
+There is no training code for CNN model.
+
+### Note
+
+This package makes use of three external codes.
+The trained files are provided by apollo. The trained files are automatically downloaded when you build.
+
+Original URL
+
+- VLP-16 :
+  <https://github.com/ApolloAuto/apollo/raw/88bfa5a1acbd20092963d6057f3a922f3939a183/modules/perception/production/data/perception/lidar/models/cnnseg/velodyne16/deploy.caffemodel>
+- HDL-64 :
+  <https://github.com/ApolloAuto/apollo/raw/88bfa5a1acbd20092963d6057f3a922f3939a183/modules/perception/production/data/perception/lidar/models/cnnseg/velodyne64/deploy.caffemodel>
+- VLS-128 :
+  <https://github.com/ApolloAuto/apollo/raw/91844c80ee4bd0cc838b4de4c625852363c258b5/modules/perception/production/data/perception/lidar/models/cnnseg/velodyne128/deploy.caffemodel>
+
+Supported lidars are velodyne 16, 64 and 128, but you can also use velodyne 32 and other lidars with good accuracy.
+
+1. [apollo 3D Obstacle Perception description](https://github.com/ApolloAuto/apollo/blob/master/docs/specs/3d_obstacle_perception.md)
+
+   ```txt
+   /******************************************************************************
+   * Copyright 2017 The Apollo Authors. All Rights Reserved.
+   *
+   * 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.
+   *****************************************************************************/
+   ```
+
+2. [tensorRTWrapper](https://github.com/lewes6369/tensorRTWrapper) :
+   It is used under the lib directory.
+
+   ```txt
+   MIT License
+
+   Copyright (c) 2018 lewes6369
+
+   Permission is hereby granted, free of charge, to any person obtaining a copy
+   of this software and associated documentation files (the "Software"), to deal
+   in the Software without restriction, including without limitation the rights
+   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+   copies of the Software, and to permit persons to whom the Software is
+   furnished to do so, subject to the following conditions:
+
+   The above copyright notice and this permission notice shall be included in all
+   copies or substantial portions of the Software.
+
+   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+   SOFTWARE.
+   ```
+
+3. [autoware_perception description](https://github.com/k0suke-murakami/autoware_perception/tree/feature/integration_baidu_seg/lidar_apollo_cnn_seg_detect)
+
+   ```txt
+   /*
+   * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+   *
+   * 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.
+   */
+   ```
+
+### Special thanks
+
+- [Apollo Project](https://github.com/ApolloAuto/apollo)
+- [lewes6369](https://github.com/lewes6369)
+- [Autoware Foundation](https://github.com/autowarefoundation/autoware)
+- [Kosuke Takeuchi](https://github.com/kosuke55) (TierIV Part timer)

perception/lidar_apollo_instance_segmentation/config/hdl-64.param.yaml

@@ -0,0 +1,8 @@
+/**:
+    ros__parameters:
+      score_threshold: 0.1
+      range: 70
+      width: 672
+      height: 672
+      use_intensity_feature: true
+      use_constant_feature: false

perception/lidar_apollo_instance_segmentation/config/vlp-16.param.yaml

@@ -0,0 +1,8 @@
+/**:
+    ros__parameters:
+      score_threshold: 0.1
+      range: 70
+      width: 672
+      height: 672
+      use_intensity_feature: true
+      use_constant_feature: false

perception/lidar_apollo_instance_segmentation/config/vls-128.param.yaml

@@ -0,0 +1,8 @@
+/**:
+    ros__parameters:
+      score_threshold: 0.1
+      range: 90
+      width: 864
+      height: 864
+      use_intensity_feature: false
+      use_constant_feature: false