diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 0dc333deef831..0f21a51062a2f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -127,8 +127,6 @@ perception/autoware_euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier perception/autoware_ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/lidar_apollo_instance_segmentation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com yoshi.ri@tier4.jp -perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com yoshi.ri@tier4.jp perception/autoware_lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp diff --git a/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt b/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt deleted file mode 100644 index 68d9e820a57d5..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt +++ /dev/null @@ -1,87 +0,0 @@ -# Copyright 2021-2022 Arm Ltd. -# -# 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. - -cmake_minimum_required(VERSION 3.14) -project(lidar_apollo_segmentation_tvm) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -set(tvm_runtime_DIR ${tvm_vendor_DIR}) -find_package(tvm_runtime CONFIG REQUIRED) -find_package(PCL 1.10 REQUIRED) - -# Used in "extras" template file -set(${PROJECT_NAME}_BUILT FALSE) - -# Gather neural network information. -set(${PROJECT_NAME}_BACKEND llvm CACHE STRING "${PROJECT_NAME} neural network backend") -set(MODEL_NAME baidu_cnn) - -# Get neural network. -set(NN_DEPENDENCY "") -get_neural_network(${MODEL_NAME} ${${PROJECT_NAME}_BACKEND} NN_DEPENDENCY) -if(NOT NN_DEPENDENCY STREQUAL "") - # Library - ament_auto_add_library(${PROJECT_NAME} SHARED - data/models/${MODEL_NAME}/inference_engine_tvm_config.hpp - include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp - include/lidar_apollo_segmentation_tvm/cluster2d.hpp - include/lidar_apollo_segmentation_tvm/disjoint_set.hpp - include/lidar_apollo_segmentation_tvm/feature_generator.hpp - include/lidar_apollo_segmentation_tvm/feature_map.hpp - include/lidar_apollo_segmentation_tvm/log_table.hpp - include/lidar_apollo_segmentation_tvm/util.hpp - src/lidar_apollo_segmentation_tvm.cpp - src/cluster2d.cpp - src/feature_generator.cpp - src/feature_map.cpp - src/log_table.cpp - ) - add_dependencies(${PROJECT_NAME} ${NN_DEPENDENCY}) - - target_compile_options(${PROJECT_NAME} PRIVATE "-Wno-sign-conversion" "-Wno-conversion") - - # Set includes as "SYSTEM" to avoid compiler errors on their headers. - target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${PCL_INCLUDE_DIRS}" - "${tvm_vendor_INCLUDE_DIRS}" - ) - - target_include_directories(${PROJECT_NAME} PRIVATE - data/models - ) - - target_link_libraries(${PROJECT_NAME} - ${PCL_LIBRARIES} - ${tf2_ros_LIBRARIES} - ${tvm_runtime_LIBRARIES} - ) - - if(BUILD_TESTING) - # gtest - set(LIDAR_APOLLO_SEGMENTATION_TVM_GTEST lidar_apollo_segmentation_tvm_gtest) - ament_add_gtest(${LIDAR_APOLLO_SEGMENTATION_TVM_GTEST} test/main.cpp TIMEOUT 120) - target_include_directories(${LIDAR_APOLLO_SEGMENTATION_TVM_GTEST} PRIVATE "include") - target_link_libraries(${LIDAR_APOLLO_SEGMENTATION_TVM_GTEST} ${PROJECT_NAME}) - endif() - - ament_export_include_directories(${PCL_INCLUDE_DIRS}) - set(${PROJECT_NAME}_BUILT TRUE) -else() - message(WARNING "Neural network not found, skipping package.") -endif() - -list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS "${PROJECT_NAME}-extras.cmake.in") -ament_auto_package() diff --git a/perception/lidar_apollo_segmentation_tvm/README.md b/perception/lidar_apollo_segmentation_tvm/README.md deleted file mode 100644 index 72fb26105e63e..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/README.md +++ /dev/null @@ -1,34 +0,0 @@ -# lidar_apollo_segmentation_tvm - -## Design - -### Usage - -#### Neural network - -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 - -The backend used for the inference can be selected by setting the `lidar_apollo_segmentation_tvm_BACKEND` cmake variable. -The current available options are `llvm` for a CPU backend, and `vulkan` for a GPU backend. -It defaults to `llvm`. - -### Convolutional Neural Networks (CNN) Segmentation - -See the [original design](https://github.com/ApolloAuto/apollo/blob/3422a62ce932cb1c0c269922a0f1aa59a290b733/docs/specs/3d_obstacle_perception.md#convolutional-neural-networks-cnn-segmentation) by Apollo. -The paragraph of interest goes up to, but excluding, the "MinBox Builder" paragraph. -This package instead relies on further processing by a dedicated shape estimator. - -Note: the parameters described in the original design have been modified and are out of date. - -### Inputs / Outputs / API - -The package exports a boolean `lidar_apollo_segmentation_tvm_BUILT` cmake variable. - -## Reference - -Lidar segmentation is based off a core algorithm by [Apollo](https://github.com/ApolloAuto/apollo/blob/r6.0.0/docs/specs/3d_obstacle_perception.md), with modifications from [TIER IV] () for the TVM backend. diff --git a/perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp b/perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp deleted file mode 100644 index 229a11645a215..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// 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 - -// cspell: ignore bcnn - -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 - - // cspell: ignore DLCPU - 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_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp deleted file mode 100644 index 170fe7feaa876..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp +++ /dev/null @@ -1,176 +0,0 @@ -// Copyright 2017-2022 Arm Ltd., TierIV, Autoware Foundation, The Apollo Authors -// -// 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. - -#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ - -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -/// \brief Internal obstacle classification categories. -enum class MetaType { - META_UNKNOWN, - META_SMALL_MOT, - META_BIG_MOT, - META_NON_MOT, - META_PEDESTRIAN, - MAX_META_TYPE -}; - -/// \brief Internal obstacle representation. -struct Obstacle -{ - std::vector grids; - pcl::PointCloud::Ptr cloud_ptr; - float score; - float height; - float heading; - MetaType meta_type; - std::vector meta_type_probabilities; - - Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(MetaType::META_UNKNOWN) - { - cloud_ptr.reset(new pcl::PointCloud); - meta_type_probabilities.assign(static_cast(MetaType::MAX_META_TYPE), 0.0); - } -}; - -/// \brief Handle the ouput of the CNN-based prediction by obtaining information on individual -/// cells. -class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D -{ -public: - /// \brief Constructor - /// \param[in] rows The number of rows in the cluster. - /// \param[in] cols The number of columns in the cluster. - /// \param[in] range Scaling factor. - explicit Cluster2D(int32_t rows, int32_t cols, float range); - - /// \brief Construct a directed graph and search the connected components for candidate object - /// clusters. - /// \param[in] inferred_data Prediction information from the neural network inference. - /// \param[in] pc_ptr Input point cloud. - /// \param[in] valid_indices Indices of the points to consider in the point cloud. - /// \param[in] objectness_thresh Threshold for filtering out non-object cells. - /// \param[in] use_all_grids_for_clustering - void cluster( - const float * inferred_data, const pcl::PointCloud::ConstPtr & pc_ptr, - const pcl::PointIndices & valid_indices, float objectness_thresh, - bool use_all_grids_for_clustering); - - /// \brief Populate the fields of obstacles_ elements. - /// \param[in] inferred_data Prediction information from the neural network inference. - void filter(const float * inferred_data); - - /// \brief Assign a classification type to the obstacles_ elements. - /// \param[in] inferred_data Prediction information from the neural network inference. - void classify(const float * inferred_data); - - /// \brief Remove the candidate clusters that don't meet the parameters' requirements. - /// \param[in] confidence_thresh The detection confidence score threshold. - /// \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. - /// \param[in] min_pts_num The candidate clusters with less than min_pts_num points are removed. - /// \return The detected objects. - std::shared_ptr getObjects( - float confidence_thresh, float height_thresh, int32_t min_pts_num); - - /// \brief Transform an obstacle from the internal representation to the external one. - /// \param[in] in_obstacle - /// \return Output obstacle. - tier4_perception_msgs::msg::DetectedObjectWithFeature obstacleToObject( - const Obstacle & in_obstacle) const; - -private: - const int32_t rows_; - const int32_t cols_; - const int32_t siz_; - const float range_; - const float scale_; - const float inv_res_x_; - const float inv_res_y_; - std::vector point2grid_; - std::vector obstacles_; - std::vector id_img_; - - pcl::PointCloud::ConstPtr pc_ptr_; - const std::vector * valid_indices_in_pc_ = nullptr; - - /// \brief Node of a directed graph. - struct Node - { - Node * center_node; - Node * parent; - int8_t node_rank; - int8_t traversed; - bool is_center; - bool is_object; - int32_t point_num; - int32_t obstacle_id; - - Node() - { - center_node = nullptr; - parent = nullptr; - node_rank = 0; - traversed = 0; - is_center = false; - is_object = false; - point_num = 0; - obstacle_id = -1; - } - }; - - /// \brief Check whether a signed row and column values are valid array indices. - inline bool IsValidRowCol(int32_t row, int32_t col) const - { - return IsValidRow(row) && IsValidCol(col); - } - - /// \brief Check whether a signed row value is a valid array index. - inline bool IsValidRow(int32_t row) const { return row >= 0 && row < rows_; } - - /// \brief Check whether a signed column value is a valid array index. - inline bool IsValidCol(int32_t col) const { return col >= 0 && col < cols_; } - - /// \brief Transform a row and column coordinate to a linear grid index. - inline int32_t RowCol2Grid(int32_t row, int32_t col) const { return row * cols_ + col; } - - /// \brief Traverse the directed graph until visiting a node. - /// \param[in] x Node to visit. - void traverse(Node * x) const; -}; -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/disjoint_set.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/disjoint_set.hpp deleted file mode 100644 index b86243a7e37e9..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/disjoint_set.hpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright 2017-2022 Arm Ltd., The Apollo Authors -// -// 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. - -#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__DISJOINT_SET_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__DISJOINT_SET_HPP_ - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -/// \brief Add a new element in a new set. -/// \param[inout] x The element to be added. -template -void DisjointSetMakeSet(T * x) -{ - x->parent = x; - x->node_rank = 0; -} - -/// \brief Recursively follow the chain of parent pointers from the input until reaching a root. -/// \param[inout] x The element which root is looked for. -/// \return The root of the set containing x. -template -T * DisjointSetFindRecursive(T * x) -{ - if (x->parent != x) { - x->parent = DisjointSetFindRecursive(x->parent); - } - return x->parent; -} - -/// \brief Find the root of the set x belongs to. -/// \param[inout] x The set element. -/// \return The root of the set containing x. -template -T * DisjointSetFind(T * x) -{ - T * y = x->parent; - if (y == x || y->parent == y) { - return y; - } - T * root = DisjointSetFindRecursive(y->parent); - x->parent = root; - y->parent = root; - return root; -} - -/// \brief Replace the set containing x and the set containing y with their union. -/// \param[inout] x An element of a first set. -/// \param[inout] y An element of a second set. -template -void DisjointSetUnion(T * x, T * y) -{ - x = DisjointSetFind(x); - y = DisjointSetFind(y); - if (x == y) { - return; - } - if (x->node_rank < y->node_rank) { - x->parent = y; - } else if (y->node_rank < x->node_rank) { - y->parent = x; - } else { - y->parent = x; - x->node_rank++; - } -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__DISJOINT_SET_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp deleted file mode 100644 index 84cf7957d2e7e..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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. - -#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ - -#include -#include -#include - -#include - -#include -#include - -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -/// \brief A FeatureMap generator based on channel feature information. -class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL FeatureGenerator -{ -private: - const float min_height_; - const float max_height_; - std::shared_ptr map_ptr_; - -public: - /// \brief Constructor - /// \param[in] width The number of cells in X (column) axis of the 2D grid. - /// \param[in] height The number of cells in Y (row) axis of the 2D grid. - /// \param[in] range The range of the 2D grid. - /// \param[in] use_intensity_feature Enable input channel intensity feature. - /// \param[in] use_constant_feature Enable input channel constant feature. - /// \param[in] min_height The minimum height. - /// \param[in] max_height The maximum height. - explicit FeatureGenerator( - int32_t width, int32_t height, int32_t range, bool use_intensity_feature, - bool use_constant_feature, float min_height, float max_height); - - /// \brief Generate a FeatureMap based on the configured features of this object. - /// \param[in] pc_ptr Pointcloud used to populate the generated FeatureMap. - /// \return A shared pointer to the generated FeatureMap. - std::shared_ptr generate( - const pcl::PointCloud::ConstPtr & pc_ptr); -}; -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp deleted file mode 100644 index f78f11c8aed56..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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. - -#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ - -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -/// \brief Abstract interface for FeatureMap. -struct FeatureMapInterface -{ -public: - const int32_t channels; - const int32_t width; - const int32_t height; - const int32_t range; - float * max_height_data; // channel 0 - float * mean_height_data; // channel 1 - float * count_data; // channel 2 - float * direction_data; // channel 3 - float * top_intensity_data; // channel 4 - float * mean_intensity_data; // channel 5 - float * distance_data; // channel 6 - float * nonempty_data; // channel 7 - std::vector map_data; - virtual void initializeMap(std::vector & map) = 0; - virtual void resetMap(std::vector & map) = 0; - explicit FeatureMapInterface(int32_t _channels, int32_t _width, int32_t _height, int32_t _range); - virtual ~FeatureMapInterface() {} -}; - -/// \brief FeatureMap with no extra feature channels. -struct FeatureMap : public FeatureMapInterface -{ - explicit FeatureMap(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; -}; - -/// \brief FeatureMap with an intensity feature channel. -struct FeatureMapWithIntensity : public FeatureMapInterface -{ - explicit FeatureMapWithIntensity(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; -}; - -/// \brief FeatureMap with a constant feature channel. -struct FeatureMapWithConstant : public FeatureMapInterface -{ - explicit FeatureMapWithConstant(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; -}; - -/// \brief FeatureMap with constant and intensity feature channels. -struct FeatureMapWithConstantAndIntensity : public FeatureMapInterface -{ - explicit FeatureMapWithConstantAndIntensity(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; -}; -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp deleted file mode 100644 index 044f319c4d335..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp +++ /dev/null @@ -1,200 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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. - -#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ - -#include -#include -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -using tier4_perception_msgs::msg::DetectedObjectsWithFeature; -using tvm_utility::pipeline::TVMArrayContainer; -using tvm_utility::pipeline::TVMArrayContainerVector; - -/// \brief Pre-precessing step of the TVM pipeline. -class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPreProcessor -: public tvm_utility::pipeline::PreProcessor::ConstPtr> -{ -public: - /// \brief Constructor. - /// \param[in] config The TVM configuration. - /// \param[in] range The range of the 2D grid. - /// \param[in] use_intensity_feature Enable input channel intensity feature. - /// \param[in] use_constant_feature Enable input channel constant feature. - /// \param[in] min_height The minimum height. - /// \param[in] max_height The maximum height. - explicit ApolloLidarSegmentationPreProcessor( - const tvm_utility::pipeline::InferenceEngineTVMConfig & config, int32_t range, - bool use_intensity_feature, bool use_constant_feature, float min_height, float max_height); - - /// \brief Transfer the input data to a TVM array. - /// \param[in] pc_ptr Input pointcloud. - /// \return A TVM array containing the pointcloud data. - /// \throw std::runtime_error If the features are incorrectly configured. - TVMArrayContainerVector schedule(const pcl::PointCloud::ConstPtr & pc_ptr); - -private: - const int64_t input_channels; - const int64_t input_width; - const int64_t input_height; - const int64_t input_datatype_bytes; - const std::shared_ptr feature_generator; - TVMArrayContainer output; -}; - -/// \brief Post-precessing step of the TVM pipeline. -class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPostProcessor -: public tvm_utility::pipeline::PostProcessor> -{ -public: - /// \brief Constructor. - /// \param[in] config The TVM configuration. - /// \param[in] pc_ptr Pointcloud containing the initial input information to be matched against - /// the result of the inference. - /// \param[in] range The range of the 2D grid. - /// \param[in] objectness_thresh The threshold of objectness for filtering out non-object cells. - /// \param[in] score_threshold The detection confidence score threshold for filtering out the - /// candidate clusters. - /// \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. - /// \param[in] min_pts_num The candidate clusters with less than min_pts_num points are removed. - explicit ApolloLidarSegmentationPostProcessor( - const tvm_utility::pipeline::InferenceEngineTVMConfig & config, - const pcl::PointCloud::ConstPtr & pc_ptr, int32_t range, - float objectness_thresh, float score_threshold, float height_thresh, int32_t min_pts_num); - - /// \brief Copy the inference result. - /// \param[in] input The result of the inference engine. - /// \return The inferred data. - std::shared_ptr schedule(const TVMArrayContainerVector & input); - -private: - const int64_t output_channels; - const int64_t output_width; - const int64_t output_height; - const int64_t output_datatype_bytes; - const float objectness_thresh_; - const float score_threshold_; - const float height_thresh_; - const int32_t min_pts_num_; - const pcl::PointCloud::ConstPtr pc_ptr_; - const std::shared_ptr cluster2d_; -}; - -/// \brief Handle the neural network inference over the input point cloud. -class LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC ApolloLidarSegmentation -{ -public: - /// \brief Constructor - /// \param[in] range The range of the 2D grid. - /// \param[in] score_threshold The detection confidence score threshold for filtering out the - /// candidate clusters in the post-processing step. - /// \param[in] use_intensity_feature Enable input channel intensity feature. - /// \param[in] use_constant_feature Enable input channel constant feature. - /// \param[in] z_offset The offset to translate up the input pointcloud before the inference. - /// \param[in] min_height The minimum height. - /// \param[in] max_height The maximum height. - /// \param[in] objectness_thresh The threshold of objectness for filtering out non-object cells in - /// the obstacle clustering step. - /// \param[in] min_pts_num In the post-processing step, the candidate clusters with less than - /// min_pts_num points are removed. - /// \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, const std::string & data_path); - - /// \brief Detect obstacles. - /// \param[in] input Input pointcloud. - /// \return Detected obstacles. - /// \throw tf2::TransformException If the pointcloud transformation fails. - /// \throw std::runtime_error If the features are incorrectly configured. - std::shared_ptr detectDynamicObjects( - const sensor_msgs::msg::PointCloud2 & input); - - /// \brief Get the name of the neural network used. - /// \return The name. - const std::string & network_name() const; - - /// \brief Check the model's version against supported versions. - /// \return The version status. - tvm_utility::Version version_check() const; - -private: - const float z_offset_; - const pcl::PointCloud::Ptr pcl_pointcloud_ptr_; - // Earliest supported model version. - const std::array model_version_from{2, 0, 0}; - - // Pipeline - using PrePT = ApolloLidarSegmentationPreProcessor; - using IET = tvm_utility::pipeline::InferenceEngineTVM; - using PostPT = ApolloLidarSegmentationPostProcessor; - - const std::shared_ptr PreP; - const std::shared_ptr IE; - const std::shared_ptr PostP; - - const std::shared_ptr> pipeline; - - /// \brief Move up the input pointcloud by z_offset_ and transform the pointcloud to target_frame_ - /// if needed. - /// \param[in] input - /// \param[out] transformed_cloud - /// \throw tf2::TransformException If the pointcloud transformation fails. - void LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL transformCloud( - const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, - float z_offset); - - rclcpp::Clock::SharedPtr clock_ = std::make_shared(RCL_ROS_TIME); - std::unique_ptr tf_buffer_ = std::make_unique(clock_); - - const std::string target_frame_ = "base_link"; -}; -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp deleted file mode 100644 index 85502267090e2..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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. - -#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -/// \brief Use a lookup table to compute the natural logarithm of 1+num. -/// \param[in] num -/// \return ln(1+num) -float calcApproximateLog(float num); -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp deleted file mode 100644 index e8e53d0bb30f2..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2017-2022 Arm Ltd., Autoware Foundation, The Apollo Authors -// -// 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. - -#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ - -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -/// \brief Project a point from a pointcloud to a 2D map. -/// \param[in] val Coordinate of the point in the pointcloud. -/// \param[in] ori Diameter of area containing the pointcloud. -/// \param[in] scale Scaling factor from pointcloud size to grid size. -/// \return The grid in which the point is. -inline int32_t F2I(float val, float ori, float scale) -{ - return static_cast(std::floor((ori - val) * scale)); -} - -/// \brief Transform a pointcloud scale to a pixel scale. -/// \param[in] in_pc Coordinate of the point in the pointcloud. -/// \param[in] in_range Range of the pointcloud. -/// \param[in] out_size Size of the grid. -/// \return The distance to the point in pixel scale. -inline int32_t Pc2Pixel(float in_pc, float in_range, float out_size) -{ - float inv_res = 0.5f * out_size / in_range; - return static_cast(std::floor((in_range - in_pc) * inv_res)); -} - -/// \brief Transform a pixel scale to a pointcloud scale. -/// \param[in] in_pixel Coordinate of the cell in the grid. -/// \param[in] in_size Size of the grid. -/// \param[in] out_range Range of the pointcloud. -/// \return The distance to the cell in pointcloud scale. -inline float Pixel2Pc(int32_t in_pixel, float in_size, float out_range) -{ - float res = 2.0f * out_range / in_size; - return out_range - (static_cast(in_pixel) + 0.5f) * res; -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware - -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/visibility_control.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/visibility_control.hpp deleted file mode 100644 index 9696e7983c2ad..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021-2022 Arm Ltd. -// -// 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. - -#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__VISIBILITY_CONTROL_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__VISIBILITY_CONTROL_HPP_ - -#if defined(__WIN32) -#if defined(LIDAR_APOLLO_SEGMENTATION_TVM_BUILDING_DLL) || \ - defined(LIDAR_APOLLO_SEGMENTATION_TVM_EXPORTS) -#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __declspec(dllexport) -#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL -#else -#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __declspec(dllimport) -#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL -#endif -#elif defined(__linux__) -#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __attribute__((visibility("default"))) -#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __attribute__((visibility("default"))) -#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm-extras.cmake.in b/perception/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm-extras.cmake.in deleted file mode 100644 index 2ecef15eb4d26..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm-extras.cmake.in +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright 2022 Arm Ltd. -# -# 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. - -set(lidar_apollo_segmentation_tvm_BUILT @lidar_apollo_segmentation_tvm_BUILT@) diff --git a/perception/lidar_apollo_segmentation_tvm/package.xml b/perception/lidar_apollo_segmentation_tvm/package.xml deleted file mode 100755 index ca4f3d2645cb9..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - lidar_apollo_segmentation_tvm - 0.1.0 - lidar_apollo_segmentation_tvm - Ambroise Vincent - Yoshi Ri - Apache 2.0 - - ament_cmake_auto - autoware_cmake - - libpcl-all-dev - - autoware_perception_msgs - autoware_universe_utils - geometry_msgs - pcl_conversions - rclcpp - sensor_msgs - tf2_eigen - tf2_geometry_msgs - tf2_ros - tier4_perception_msgs - tvm_utility - tvm_vendor - - ament_cmake_gtest - ament_lint_auto - autoware_lint_common - libpcl-all-dev - - - ament_cmake - - diff --git a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp deleted file mode 100644 index db2068b978a2e..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp +++ /dev/null @@ -1,361 +0,0 @@ -// Copyright 2017-2022 Arm Ltd., TierIV, Autoware Foundation, The Apollo Authors -// -// 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 - -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include -#include - -using Point = geometry_msgs::msg::Point32; - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -geometry_msgs::msg::Quaternion getQuaternionFromRPY(const double r, const double p, const double y) -{ - tf2::Quaternion q; - q.setRPY(r, p, y); - return tf2::toMsg(q); -} - -Cluster2D::Cluster2D(const int32_t rows, const int32_t cols, const float range) -: rows_(rows), - cols_(cols), - siz_(rows * cols), - range_(range), - scale_(0.5f * static_cast(rows) / range), - inv_res_x_(0.5f * static_cast(cols) / range), - inv_res_y_(0.5f * static_cast(rows) / range) -{ - point2grid_.clear(); - id_img_.assign(siz_, -1); - pc_ptr_.reset(); - valid_indices_in_pc_ = nullptr; -} - -void Cluster2D::traverse(Node * x) const -{ - std::vector p; - p.clear(); - - while (x->traversed == 0) { - p.push_back(x); - x->traversed = 2; - x = x->center_node; - } - if (x->traversed == 2) { - for (int i = static_cast(p.size()) - 1; i >= 0 && p[i] != x; i--) { - p[i]->is_center = true; - } - x->is_center = true; - } - for (size_t i = 0; i < p.size(); i++) { - Node * y = p[i]; - y->traversed = 1; - y->parent = x->parent; - } -} - -void Cluster2D::cluster( - const float * inferred_data, const pcl::PointCloud::ConstPtr & pc_ptr, - const pcl::PointIndices & valid_indices, float objectness_thresh, - bool use_all_grids_for_clustering) -{ - const float * category_pt_data = inferred_data; - const float * instance_pt_x_data = inferred_data + siz_; - const float * instance_pt_y_data = inferred_data + siz_ * 2; - - pc_ptr_ = pc_ptr; - - std::vector> nodes(rows_, std::vector(cols_, Node())); - - valid_indices_in_pc_ = &(valid_indices.indices); - point2grid_.assign(valid_indices_in_pc_->size(), -1); - - for (size_t i = 0; i < valid_indices_in_pc_->size(); ++i) { - int32_t point_id = valid_indices_in_pc_->at(i); - const auto & point = pc_ptr_->points[point_id]; - // * the coordinates of x and y have been exchanged in feature generation - // step, - // so we swap them back here. - int32_t pos_x = F2I(point.y, range_, inv_res_x_); // col - int32_t pos_y = F2I(point.x, range_, inv_res_y_); // row - if (IsValidRowCol(pos_y, pos_x)) { - point2grid_[i] = RowCol2Grid(pos_y, pos_x); - nodes[pos_y][pos_x].point_num++; - } - } - - for (int32_t row = 0; row < rows_; ++row) { - for (int32_t col = 0; col < cols_; ++col) { - int32_t grid = RowCol2Grid(row, col); - Node * node = &nodes[row][col]; - DisjointSetMakeSet(node); - node->is_object = (use_all_grids_for_clustering || nodes[row][col].point_num > 0) && - (*(category_pt_data + grid) >= objectness_thresh); - int32_t center_row = - row + static_cast(std::round(instance_pt_x_data[grid] * scale_)); - int32_t center_col = - col + static_cast(std::round(instance_pt_y_data[grid] * scale_)); - center_row = std::min(std::max(center_row, 0), rows_ - 1); - center_col = std::min(std::max(center_col, 0), cols_ - 1); - node->center_node = &nodes[center_row][center_col]; - } - } - - for (int32_t row = 0; row < rows_; ++row) { - for (int32_t col = 0; col < cols_; ++col) { - Node * node = &nodes[row][col]; - if (node->is_object && node->traversed == 0) { - traverse(node); - } - } - } - - for (int32_t row = 0; row < rows_; ++row) { - for (int32_t col = 0; col < cols_; ++col) { - Node * node = &nodes[row][col]; - if (!node->is_center) { - continue; - } - for (int32_t row2 = row - 1; row2 <= row + 1; ++row2) { - for (int32_t col2 = col - 1; col2 <= col + 1; ++col2) { - if ((row2 == row || col2 == col) && IsValidRowCol(row2, col2)) { - Node * node2 = &nodes[row2][col2]; - if (node2->is_center) { - DisjointSetUnion(node, node2); - } - } - } - } - } - } - - int32_t count_obstacles = 0; - obstacles_.clear(); - id_img_.assign(siz_, -1); - for (int32_t row = 0; row < rows_; ++row) { - for (int32_t col = 0; col < cols_; ++col) { - Node * node = &nodes[row][col]; - if (!node->is_object) { - continue; - } - Node * root = DisjointSetFind(node); - if (root->obstacle_id < 0) { - root->obstacle_id = count_obstacles++; - obstacles_.push_back(Obstacle()); - } - int32_t grid = RowCol2Grid(row, col); - id_img_[grid] = root->obstacle_id; - obstacles_[root->obstacle_id].grids.push_back(grid); - } - } - filter(inferred_data); - classify(inferred_data); -} - -void Cluster2D::filter(const float * inferred_data) -{ - const float * confidence_pt_data = inferred_data + siz_ * 3; - const float * heading_pt_x_data = inferred_data + siz_ * 9; - const float * heading_pt_y_data = inferred_data + siz_ * 10; - const float * height_pt_data = inferred_data + siz_ * 11; - - for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { - Obstacle * obs = &obstacles_[obstacle_id]; - double score = 0.0; - double height = 0.0; - double vec_x = 0.0; - double vec_y = 0.0; - for (int32_t grid : obs->grids) { - score += static_cast(confidence_pt_data[grid]); - height += static_cast(height_pt_data[grid]); - vec_x += heading_pt_x_data[grid]; - vec_y += heading_pt_y_data[grid]; - } - obs->score = static_cast(score / static_cast(obs->grids.size())); - obs->height = static_cast(height / static_cast(obs->grids.size())); - obs->heading = static_cast(std::atan2(vec_y, vec_x) * 0.5); - obs->cloud_ptr.reset(new pcl::PointCloud); - } -} - -void Cluster2D::classify(const float * inferred_data) -{ - const float * classify_pt_data = inferred_data + siz_ * 4; - int num_classes = static_cast(MetaType::MAX_META_TYPE); - for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++) { - Obstacle * obs = &obstacles_[obs_id]; - - for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) { - int32_t grid = obs->grids[grid_id]; - for (int k = 0; k < num_classes; k++) { - obs->meta_type_probabilities[k] += classify_pt_data[k * siz_ + grid]; - } - } - int meta_type_id = 0; - for (int k = 0; k < num_classes; k++) { - obs->meta_type_probabilities[k] /= static_cast(obs->grids.size()); - if (obs->meta_type_probabilities[k] > obs->meta_type_probabilities[meta_type_id]) { - meta_type_id = k; - } - } - obs->meta_type = static_cast(meta_type_id); - } -} - -tier4_perception_msgs::msg::DetectedObjectWithFeature Cluster2D::obstacleToObject( - const Obstacle & in_obstacle) const -{ - using autoware_perception_msgs::msg::DetectedObjectKinematics; - using autoware_perception_msgs::msg::ObjectClassification; - - tier4_perception_msgs::msg::DetectedObjectWithFeature resulting_object; - - resulting_object.object.classification.emplace_back( - autoware_perception_msgs::build() - .label(ObjectClassification::UNKNOWN) - .probability(in_obstacle.score)); - if (in_obstacle.meta_type == MetaType::META_PEDESTRIAN) { - resulting_object.object.classification.front().label = ObjectClassification::PEDESTRIAN; - } else if (in_obstacle.meta_type == MetaType::META_NON_MOT) { - resulting_object.object.classification.front().label = ObjectClassification::MOTORCYCLE; - } else if (in_obstacle.meta_type == MetaType::META_SMALL_MOT) { - resulting_object.object.classification.front().label = ObjectClassification::CAR; - } else if (in_obstacle.meta_type == MetaType::META_BIG_MOT) { - resulting_object.object.classification.front().label = ObjectClassification::BUS; - } else { - resulting_object.object.classification.front().label = ObjectClassification::UNKNOWN; - } - - pcl::PointXYZ min_point; - pcl::PointXYZ max_point; - for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end(); - ++pit) { - if (pit->x < min_point.x) { - min_point.x = pit->x; - } - if (pit->y < min_point.y) { - min_point.y = pit->y; - } - if (pit->z < min_point.z) { - min_point.z = pit->z; - } - if (pit->x > max_point.x) { - max_point.x = pit->x; - } - if (pit->y > max_point.y) { - max_point.y = pit->y; - } - if (pit->z > max_point.z) { - max_point.z = pit->z; - } - } - - // cluster and ground filtering - pcl::PointCloud cluster; - const float min_height = min_point.z + ((max_point.z - min_point.z) * 0.1f); - for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end(); - ++pit) { - if (min_height < pit->z) { - cluster.points.push_back(*pit); - } - } - min_point.z = 0.0; - max_point.z = 0.0; - for (auto pit = cluster.points.begin(); pit != cluster.points.end(); ++pit) { - if (pit->z < min_point.z) { - min_point.z = pit->z; - } - if (pit->z > max_point.z) { - max_point.z = pit->z; - } - } - sensor_msgs::msg::PointCloud2 ros_pc; - pcl::toROSMsg(cluster, ros_pc); - resulting_object.feature.cluster = ros_pc; - - // position - const float height = max_point.z - min_point.z; - const float length = max_point.x - min_point.x; - const float width = max_point.y - min_point.y; - resulting_object.object.kinematics.pose_with_covariance.pose.position.x = - min_point.x + length / 2; - resulting_object.object.kinematics.pose_with_covariance.pose.position.y = min_point.y + width / 2; - resulting_object.object.kinematics.pose_with_covariance.pose.position.z = - min_point.z + height / 2; - - resulting_object.object.kinematics.pose_with_covariance.pose.orientation = - getQuaternionFromRPY(0.0, 0.0, in_obstacle.heading); - resulting_object.object.kinematics.orientation_availability = - DetectedObjectKinematics::SIGN_UNKNOWN; - - return resulting_object; -} - -std::shared_ptr Cluster2D::getObjects( - const float confidence_thresh, const float height_thresh, const int32_t min_pts_num) -{ - auto object_array = std::make_shared(); - - for (size_t i = 0; i < point2grid_.size(); ++i) { - int32_t grid = point2grid_[i]; - if (grid < 0) { - continue; - } - - int32_t obstacle_id = id_img_[grid]; - - int32_t point_id = valid_indices_in_pc_->at(i); - - if (obstacle_id >= 0 && obstacles_[obstacle_id].score >= confidence_thresh) { - if ( - height_thresh < 0 || - pc_ptr_->points[point_id].z <= obstacles_[obstacle_id].height + height_thresh) { - obstacles_[obstacle_id].cloud_ptr->push_back(pc_ptr_->points[point_id]); - } - } - } - - for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { - Obstacle * obs = &obstacles_[obstacle_id]; - if (static_cast(obs->cloud_ptr->size()) < min_pts_num) { - continue; - } - tier4_perception_msgs::msg::DetectedObjectWithFeature out_obj = obstacleToObject(*obs); - object_array->feature_objects.push_back(out_obj); - } - - return object_array; -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp b/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp deleted file mode 100644 index fd038efc60ffb..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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 -#include - -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -namespace -{ -inline float normalizeIntensity(float intensity) -{ - return intensity / 255; -} -} // namespace - -FeatureGenerator::FeatureGenerator( - const int32_t width, const int32_t height, const int32_t range, const bool use_intensity_feature, - const bool use_constant_feature, const float min_height, const float max_height) -: min_height_(min_height), max_height_(max_height) -{ - // select feature map type - if (use_constant_feature && use_intensity_feature) { - map_ptr_ = std::make_shared(width, height, range); - } else if (use_constant_feature) { - map_ptr_ = std::make_shared(width, height, range); - } else if (use_intensity_feature) { - map_ptr_ = std::make_shared(width, height, range); - } else { - map_ptr_ = std::make_shared(width, height, range); - } - map_ptr_->initializeMap(map_ptr_->map_data); -} - -std::shared_ptr FeatureGenerator::generate( - const pcl::PointCloud::ConstPtr & pc_ptr) -{ - const double epsilon = 1e-6; - map_ptr_->resetMap(map_ptr_->map_data); - - const int32_t size = map_ptr_->height * map_ptr_->width; - - const float inv_res_x = 0.5f * map_ptr_->width / map_ptr_->range; - const float inv_res_y = 0.5f * map_ptr_->height / map_ptr_->range; - - for (size_t i = 0; i < pc_ptr->points.size(); ++i) { - if (pc_ptr->points[i].z <= min_height_ || max_height_ <= pc_ptr->points[i].z) { - continue; - } - - // x on grid - const int32_t pos_x = static_cast( - std::floor((static_cast(map_ptr_->range) - pc_ptr->points[i].y) * inv_res_x)); - // y on grid - const int32_t pos_y = static_cast( - std::floor((static_cast(map_ptr_->range) - pc_ptr->points[i].x) * inv_res_y)); - if (pos_x < 0 || map_ptr_->width <= pos_x || pos_y < 0 || map_ptr_->height <= pos_y) { - continue; - } - - const int32_t idx = pos_y * map_ptr_->width + pos_x; - - if (map_ptr_->max_height_data[idx] < pc_ptr->points[i].z) { - map_ptr_->max_height_data[idx] = pc_ptr->points[i].z; - if (map_ptr_->top_intensity_data != nullptr) { - map_ptr_->top_intensity_data[idx] = normalizeIntensity(pc_ptr->points[i].intensity); - } - } - map_ptr_->mean_height_data[idx] += pc_ptr->points[i].z; - if (map_ptr_->mean_intensity_data != nullptr) { - map_ptr_->mean_intensity_data[idx] += normalizeIntensity(pc_ptr->points[i].intensity); - } - map_ptr_->count_data[idx] += 1.0f; - } - - for (int32_t i = 0; i < size; ++i) { - if (static_cast(map_ptr_->count_data[i]) < epsilon) { - map_ptr_->max_height_data[i] = 0.0f; - } else { - map_ptr_->mean_height_data[i] /= map_ptr_->count_data[i]; - if (map_ptr_->mean_intensity_data != nullptr) { - map_ptr_->mean_intensity_data[i] /= map_ptr_->count_data[i]; - } - map_ptr_->nonempty_data[i] = 1.0f; - } - map_ptr_->count_data[i] = calcApproximateLog(map_ptr_->count_data[i]); - } - return map_ptr_; -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp b/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp deleted file mode 100644 index a310ddae03f77..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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 -#include - -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -FeatureMapInterface::FeatureMapInterface( - const int32_t _channels, const int32_t _width, const int32_t _height, const int32_t _range) -: channels(_channels), - width(_width), - height(_height), - range(_range), - max_height_data(nullptr), - mean_height_data(nullptr), - count_data(nullptr), - direction_data(nullptr), - top_intensity_data(nullptr), - mean_intensity_data(nullptr), - distance_data(nullptr), - nonempty_data(nullptr) -{ - map_data.resize(width * height * channels); -} - -FeatureMap::FeatureMap(const int32_t width, const int32_t height, const int32_t range) -: FeatureMapInterface(4, width, height, range) -{ - max_height_data = &(map_data[0]) + width * height * 0; - mean_height_data = &(map_data[0]) + width * height * 1; - count_data = &(map_data[0]) + width * height * 2; - nonempty_data = &(map_data[0]) + width * height * 3; -} -void FeatureMap::initializeMap(std::vector & map) -{ - (void)map; -} -void FeatureMap::resetMap(std::vector & map) -{ - const int32_t size = width * height; - (void)map; - for (int32_t i = 0; i < size; ++i) { - max_height_data[i] = -5.0f; - mean_height_data[i] = 0.0f; - count_data[i] = 0.0f; - nonempty_data[i] = 0.0f; - } -} - -FeatureMapWithIntensity::FeatureMapWithIntensity( - const int32_t width, const int32_t height, const int32_t range) -: FeatureMapInterface(6, width, height, range) -{ - max_height_data = &(map_data[0]) + width * height * 0; - mean_height_data = &(map_data[0]) + width * height * 1; - count_data = &(map_data[0]) + width * height * 2; - top_intensity_data = &(map_data[0]) + width * height * 3; - mean_intensity_data = &(map_data[0]) + width * height * 4; - nonempty_data = &(map_data[0]) + width * height * 5; -} -void FeatureMapWithIntensity::initializeMap(std::vector & map) -{ - (void)map; -} -void FeatureMapWithIntensity::resetMap(std::vector & map) -{ - const int32_t size = width * height; - (void)map; - for (int32_t i = 0; i < size; ++i) { - max_height_data[i] = -5.0f; - mean_height_data[i] = 0.0f; - count_data[i] = 0.0f; - top_intensity_data[i] = 0.0f; - mean_intensity_data[i] = 0.0f; - nonempty_data[i] = 0.0f; - } -} - -FeatureMapWithConstant::FeatureMapWithConstant( - const int32_t width, const int32_t height, const int32_t range) -: FeatureMapInterface(6, width, height, range) -{ - max_height_data = &(map_data[0]) + width * height * 0; - mean_height_data = &(map_data[0]) + width * height * 1; - count_data = &(map_data[0]) + width * height * 2; - direction_data = &(map_data[0]) + width * height * 3; - distance_data = &(map_data[0]) + width * height * 4; - nonempty_data = &(map_data[0]) + width * height * 5; -} -void FeatureMapWithConstant::initializeMap(std::vector & map) -{ - (void)map; - for (int32_t row = 0; row < height; ++row) { - for (int32_t col = 0; col < width; ++col) { - int32_t idx = row * width + col; - // * row <-> x, column <-> y - // return the distance from the car to center of the grid. - // Pc means point cloud = real world scale. so transform pixel scale to - // real world scale - float center_x = Pixel2Pc(row, height, range); - float center_y = Pixel2Pc(col, width, range); - // normalization. -0.5~0.5 - direction_data[idx] = std::atan2(center_y, center_x) / static_cast(2.0 * M_PI); - distance_data[idx] = std::hypot(center_x, center_y) / 60.0f - 0.5f; - } - } -} - -void FeatureMapWithConstant::resetMap(std::vector & map) -{ - const int32_t size = width * height; - (void)map; - for (int32_t i = 0; i < size; ++i) { - max_height_data[i] = -5.0f; - mean_height_data[i] = 0.0f; - count_data[i] = 0.0f; - nonempty_data[i] = 0.0f; - } -} - -FeatureMapWithConstantAndIntensity::FeatureMapWithConstantAndIntensity( - const int32_t width, const int32_t height, const int32_t range) -: FeatureMapInterface(8, width, height, range) -{ - max_height_data = &(map_data[0]) + width * height * 0; - mean_height_data = &(map_data[0]) + width * height * 1; - count_data = &(map_data[0]) + width * height * 2; - direction_data = &(map_data[0]) + width * height * 3; - top_intensity_data = &(map_data[0]) + width * height * 4; - mean_intensity_data = &(map_data[0]) + width * height * 5; - distance_data = &(map_data[0]) + width * height * 6; - nonempty_data = &(map_data[0]) + width * height * 7; -} -void FeatureMapWithConstantAndIntensity::initializeMap(std::vector & map) -{ - (void)map; - for (int32_t row = 0; row < height; ++row) { - for (int32_t col = 0; col < width; ++col) { - int32_t idx = row * width + col; - // * row <-> x, column <-> y - // return the distance from the car to center of the grid. - // Pc means point cloud = real world scale. so transform pixel scale to - // real world scale - float center_x = Pixel2Pc(row, height, range); - float center_y = Pixel2Pc(col, width, range); - // normalization. -0.5~0.5 - direction_data[idx] = std::atan2(center_y, center_x) / static_cast(2.0 * M_PI); - distance_data[idx] = std::hypot(center_x, center_y) / 60.0f - 0.5f; - } - } -} - -void FeatureMapWithConstantAndIntensity::resetMap(std::vector & map) -{ - const int32_t size = width * height; - (void)map; - for (int32_t i = 0; i < size; ++i) { - max_height_data[i] = -5.0f; - mean_height_data[i] = 0.0f; - count_data[i] = 0.0f; - top_intensity_data[i] = 0.0f; - mean_intensity_data[i] = 0.0f; - nonempty_data[i] = 0.0f; - } -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp deleted file mode 100644 index 32211bf84efec..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ /dev/null @@ -1,225 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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 -#include -#include -#include -#include - -#include - -#include -#include -#include - -// cspell: ignore bcnn -using model_zoo::perception::lidar_obstacle_detection::baidu_cnn::onnx_bcnn::config; - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -ApolloLidarSegmentationPreProcessor::ApolloLidarSegmentationPreProcessor( - const tvm_utility::pipeline::InferenceEngineTVMConfig & config, int32_t range, - bool use_intensity_feature, bool use_constant_feature, float min_height, float max_height) -: input_channels(config.network_inputs[0].node_shape[1]), - input_width(config.network_inputs[0].node_shape[2]), - input_height(config.network_inputs[0].node_shape[3]), - input_datatype_bytes(config.network_inputs[0].tvm_dtype_bits / 8), - feature_generator(std::make_shared( - input_width, input_height, range, use_intensity_feature, use_constant_feature, min_height, - max_height)) -{ - // Allocate input variable - std::vector shape_x{1, input_channels, input_width, input_height}; - TVMArrayContainer x{ - shape_x, - config.network_inputs[0].tvm_dtype_code, - config.network_inputs[0].tvm_dtype_bits, - config.network_inputs[0].tvm_dtype_lanes, - config.tvm_device_type, - config.tvm_device_id}; - output = x; -} - -TVMArrayContainerVector ApolloLidarSegmentationPreProcessor::schedule( - const pcl::PointCloud::ConstPtr & pc_ptr) -{ - // generate feature map - std::shared_ptr feature_map_ptr = feature_generator->generate(pc_ptr); - - if (feature_map_ptr->channels < input_channels) { - throw std::runtime_error("schedule: incorrect feature configuration"); - } - - TVMArrayCopyFromBytes( - output.getArray(), feature_map_ptr->map_data.data(), - input_channels * input_height * input_width * input_datatype_bytes); - - return {output}; -} - -ApolloLidarSegmentationPostProcessor::ApolloLidarSegmentationPostProcessor( - const tvm_utility::pipeline::InferenceEngineTVMConfig & config, - const pcl::PointCloud::ConstPtr & pc_ptr, int32_t range, float objectness_thresh, - float score_threshold, float height_thresh, int32_t min_pts_num) -: output_channels(config.network_outputs[0].node_shape[1]), - output_width(config.network_outputs[0].node_shape[2]), - output_height(config.network_outputs[0].node_shape[3]), - output_datatype_bytes(config.network_outputs[0].tvm_dtype_bits / 8), - objectness_thresh_(objectness_thresh), - score_threshold_(score_threshold), - height_thresh_(height_thresh), - min_pts_num_(min_pts_num), - pc_ptr_(pc_ptr), - cluster2d_(std::make_shared(output_width, output_height, range)) -{ -} - -std::shared_ptr ApolloLidarSegmentationPostProcessor::schedule( - const TVMArrayContainerVector & input) -{ - pcl::PointIndices valid_idx; - valid_idx.indices.resize(pc_ptr_->size()); - std::iota(valid_idx.indices.begin(), valid_idx.indices.end(), 0); - std::vector feature(output_channels * output_width * output_height, 0); - TVMArrayCopyToBytes( - input[0].getArray(), feature.data(), - output_channels * output_width * output_height * output_datatype_bytes); - cluster2d_->cluster( - feature.data(), pc_ptr_, valid_idx, objectness_thresh_, true /*use all grids for clustering*/); - auto object_array = cluster2d_->getObjects(score_threshold_, height_thresh_, min_pts_num_); - - return object_array; -} - -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, const std::string & data_path) -: z_offset_(z_offset), - pcl_pointcloud_ptr_(new pcl::PointCloud), - PreP(std::make_shared( - config, range, use_intensity_feature, use_constant_feature, min_height, max_height)), - IE(std::make_shared(config, "lidar_apollo_segmentation_tvm", data_path)), - PostP(std::make_shared( - config, pcl_pointcloud_ptr_, range, objectness_thresh, score_threshold, height_thresh, - min_pts_num)), - pipeline( - std::make_shared>(*PreP, *IE, *PostP)) -{ -} - -void ApolloLidarSegmentation::transformCloud( - const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, - float z_offset) -{ - if (target_frame_ == input.header.frame_id && z_offset == 0) { - transformed_cloud = input; - } else { - pcl::PointCloud in_cluster, transformed_cloud_cluster; - pcl::fromROSMsg(input, in_cluster); - - // transform pointcloud to target_frame - if (target_frame_ != input.header.frame_id) { - geometry_msgs::msg::TransformStamped transform_stamped; - builtin_interfaces::msg::Time time_stamp = input.header.stamp; - const tf2::TimePoint time_point = tf2::TimePoint( - std::chrono::seconds(time_stamp.sec) + std::chrono::nanoseconds(time_stamp.nanosec)); - transform_stamped = - tf_buffer_->lookupTransform(target_frame_, input.header.frame_id, time_point); - Eigen::Matrix4f affine_matrix = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - autoware::universe_utils::transformPointCloud( - in_cluster, transformed_cloud_cluster, affine_matrix); - transformed_cloud_cluster.header.frame_id = target_frame_; - } else { - transformed_cloud_cluster = in_cluster; - } - - // move pointcloud z_offset in z axis - if (z_offset != 0) { - Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); - Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - autoware::universe_utils::transformPointCloud( - transformed_cloud_cluster, transformed_cloud_cluster, z_up_transform); - } - - pcl::toROSMsg(transformed_cloud_cluster, transformed_cloud); - } -} - -std::shared_ptr ApolloLidarSegmentation::detectDynamicObjects( - const sensor_msgs::msg::PointCloud2 & input) -{ - // move up pointcloud z_offset in z axis - sensor_msgs::msg::PointCloud2 transformed_cloud; - ApolloLidarSegmentation::transformCloud(input, transformed_cloud, z_offset_); - // convert from ros to pcl - // pcl::fromROSMsg( - // transformed_cloud, *pcl_pointcloud_ptr_); // Manual conversion is needed since intensity - // comes as an uint8_t - - auto pcl_pointcloud = *pcl_pointcloud_ptr_; - pcl_pointcloud.width = input.width; - pcl_pointcloud.height = input.height; - pcl_pointcloud.is_dense = input.is_dense == 1; - pcl_pointcloud.resize(input.width * input.height); - - sensor_msgs::PointCloud2ConstIterator it_x(input, "x"); - sensor_msgs::PointCloud2ConstIterator it_y(input, "y"); - sensor_msgs::PointCloud2ConstIterator it_z(input, "z"); - sensor_msgs::PointCloud2ConstIterator it_intensity(input, "intensity"); - - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) { - pcl::PointXYZI point; - point.x = *it_x; - point.y = *it_y; - point.z = *it_z; - point.intensity = static_cast(*it_intensity); - pcl_pointcloud.emplace_back(std::move(point)); - } - - // inference pipeline - auto output = pipeline->schedule(pcl_pointcloud_ptr_); - for (auto & feature_object : output->feature_objects) { - feature_object.feature.cluster.header = input.header; - } - output->header = input.header; - - // move down pointcloud z_offset in z axis - for (auto & feature_object : output->feature_objects) { - sensor_msgs::msg::PointCloud2 updated_cloud; - transformCloud(feature_object.feature.cluster, updated_cloud, -z_offset_); - feature_object.feature.cluster = updated_cloud; - } - - return output; -} - -const std::string & ApolloLidarSegmentation::network_name() const -{ - return config.network_name; -} - -tvm_utility::Version ApolloLidarSegmentation::version_check() const -{ - return IE->version_check(model_version_from); -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp b/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp deleted file mode 100644 index de2f1734e0733..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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 - -#include -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -struct LogTable -{ - std::vector data; - LogTable() - { - data.resize(256 * 10); - for (size_t i = 0; i < data.size(); ++i) { - data[i] = std::log1p(static_cast(i) / 10); - } - } -}; - -static LogTable log_table; - -float calcApproximateLog(float num) -{ - if (num >= 0) { - size_t integer_num = static_cast(num * 10.0f); - if (integer_num < log_table.data.size()) { - return log_table.data[integer_num]; - } - } - return std::log1p(num); -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/test/main.cpp b/perception/lidar_apollo_segmentation_tvm/test/main.cpp deleted file mode 100644 index 06cff2a67e908..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/test/main.cpp +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright 2021-2022 Arm Ltd. -// -// 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 -#include - -#include - -#include -#include -#include -#include -#include - -using autoware::perception::lidar_apollo_segmentation_tvm::ApolloLidarSegmentation; -namespace fs = std::filesystem; - -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; - const int height = 10000; - const int range = 70; - const float score_threshold = 0.8f; - const float z_offset = 1.0f; - const float min_height = -5.0f; - const float max_height = 5.0f; - 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, data_path); - - auto version_status = segmentation.version_check(); - EXPECT_NE(version_status, tvm_utility::Version::Unsupported); - - std::random_device rd; - std::mt19937 gen(42); - std::uniform_real_distribution dis(-50.0, 50.0); - std::vector v(width * height * sizeof(float) * 4); - for (size_t i = 0; i < width * height * 4; i++) { - reinterpret_cast(v.data())[i] = dis(gen); - } - - sensor_msgs::msg::PointCloud2 input{}; - input.header.frame_id = "base_link"; - input.fields.resize(4U); - input.fields[0U].name = "x"; - input.fields[1U].name = "y"; - input.fields[2U].name = "z"; - input.fields[3U].name = "intensity"; - for (uint32_t idx = 0U; idx < 4U; ++idx) { - input.fields[idx].offset = static_cast(idx * sizeof(float)); - input.fields[idx].datatype = sensor_msgs::msg::PointField::FLOAT32; - input.fields[idx].count = 1U; - input.point_step += static_cast(sizeof(float)); - } - input.height = static_cast(height); - input.width = static_cast(width); - input.is_bigendian = false; - input.row_step = input.point_step * input.width; - input.is_dense = false; - input.data = v; - - std::shared_ptr output; - bool has_thrown = false; - try { - output = segmentation.detectDynamicObjects(input); - } catch (const std::exception & e) { - has_thrown = true; - } - EXPECT_EQ(expect_throw, has_thrown); -} - -// Other test configurations to increase code coverage. -TEST(lidar_apollo_segmentation_tvm, others) -{ - 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); -} diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt b/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt deleted file mode 100644 index 100e14277ec1b..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt +++ /dev/null @@ -1,52 +0,0 @@ -# Copyright 2021-2023 Arm Ltd. -# -# 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. - -cmake_minimum_required(VERSION 3.14) -project(lidar_apollo_segmentation_tvm_nodes) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -# Only build if the "core" package has been built. -if(lidar_apollo_segmentation_tvm_BUILT) - # Set lidar_apollo_segmentation_tvm includes as "SYSTEM" to ignore compiler errors on PCL headers - include_directories(SYSTEM "${lidar_apollo_segmentation_tvm_INCLUDE_DIRS}") - - # Library - ament_auto_add_library(${PROJECT_NAME} SHARED - include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp - src/lidar_apollo_segmentation_tvm_node.cpp - ) - target_link_libraries(${PROJECT_NAME} ${lidar_apollo_segmentation_tvm_LIBRARIES}) - - rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "autoware::perception::lidar_apollo_segmentation_tvm_nodes::ApolloLidarSegmentationNode" - EXECUTABLE ${PROJECT_NAME}_exe - ) - - if(BUILD_TESTING) - add_ros_test( - test/launch.test.py - TIMEOUT "30" - ) - endif() - - ament_auto_package(INSTALL_TO_SHARE - launch - config - ) -else() - message(WARNING "lidar_apollo_segmentation_tvm not built, skipping package.") - ament_auto_package() -endif() diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/README.md b/perception/lidar_apollo_segmentation_tvm_nodes/README.md deleted file mode 100644 index a9ffd59f228ec..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/README.md +++ /dev/null @@ -1,83 +0,0 @@ - - -# lidar_apollo_segmentation_tvm_nodes - -## Purpose / Use cases - -An alternative to Euclidean clustering. -This node detects and labels foreground obstacles (e.g. cars, motorcycles, pedestrians) from a point -cloud, using a neural network. - -## Design - -See the design of the algorithm in the core (lidar_apollo_segmentation_tvm) package's design documents. - -### Usage - -`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 - -The original node from Apollo has a Region Of Interest (ROI) filter. -This has the benefit of working with a filtered point cloud that includes only the points inside the -ROI (i.e., the drivable road and junction areas) with most of the background obstacles removed (such -as buildings and trees around the road region). -Not having this filter may negatively impact performance. - -### Inputs / Outputs / API - -#### Inputs - -The input are non-ground points as a PointCloud2 message from the sensor_msgs package. - -#### Outputs - -The output is a [DetectedObjectsWithFeature](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/object_recognition/DetectedObjectsWithFeature.msg). - -#### Parameters - -{{ json_to_markdown("perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json") }} - -### Error detection and handling - -Abort and warn when the input frame can't be converted to `base_link`. - -### Security considerations - -Both the input and output are controlled by the same actor, so the following security concerns are -out-of-scope: - -- Spoofing -- Tampering - -Leaking data to another actor would require a flaw in TVM or the host operating system that allows -arbitrary memory to be read, a significant security flaw in itself. -This is also true for an external actor operating the pipeline early: only the object that initiated -the pipeline can run the methods to receive its output. - -A Denial-of-Service attack could make the target hardware unusable for other pipelines but would -require being able to run code on the CPU, which would already allow a more severe Denial-of-Service -attack. - -No elevation of privilege is required for this package. - -## Future extensions / Unimplemented parts - -## Related issues - -- #226: Autoware.Auto Neural Networks Inference Architecture Design diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/config/lidar_apollo_segmentation_tvm_nodes.param.yaml b/perception/lidar_apollo_segmentation_tvm_nodes/config/lidar_apollo_segmentation_tvm_nodes.param.yaml deleted file mode 100644 index 30bf0ba68d28c..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/config/lidar_apollo_segmentation_tvm_nodes.param.yaml +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2023 Arm Ltd. -# -# 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. - -/**: - ros__parameters: - range: 90 - score_threshold: 0.1 - use_intensity_feature: false - use_constant_feature: false - z_offset: 0.0 - min_height: -5.0 - max_height: 5.0 - objectness_thresh: 0.5 - min_pts_num: 3 - height_thresh: 0.5 - data_path: $(env HOME)/autoware_data diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp b/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp deleted file mode 100644 index eb05ca28c3a39..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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. - -#ifndef LIDAR_APOLLO_SEGMENTATION_TVM_NODES__LIDAR_APOLLO_SEGMENTATION_TVM_NODE_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES__LIDAR_APOLLO_SEGMENTATION_TVM_NODE_HPP_ - -#include -#include -#include -#include -#include - -#include - -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm_nodes -{ -/// \brief Object detection node based on neural network inference. -class LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC ApolloLidarSegmentationNode : public rclcpp::Node -{ -private: - const rclcpp::Subscription::SharedPtr m_cloud_sub_ptr; - const rclcpp::Publisher::SharedPtr - m_detected_pub_ptr; - const std::shared_ptr m_detector_ptr; - /// \brief Main callback function. - void LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL - pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr & msg); - -public: - /// \brief Constructor - /// \param options Additional options to control creation of the node. - explicit ApolloLidarSegmentationNode(const rclcpp::NodeOptions & options); -}; -} // namespace lidar_apollo_segmentation_tvm_nodes -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM_NODES__LIDAR_APOLLO_SEGMENTATION_TVM_NODE_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/visibility_control.hpp b/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/visibility_control.hpp deleted file mode 100644 index 0521737a96ed1..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021-2022 Arm Ltd. -// -// 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. - -#ifndef LIDAR_APOLLO_SEGMENTATION_TVM_NODES__VISIBILITY_CONTROL_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES__VISIBILITY_CONTROL_HPP_ - -#if defined(__WIN32) -#if defined(LIDAR_APOLLO_SEGMENTATION_TVM_NODES_BUILDING_DLL) || \ - defined(LIDAR_APOLLO_SEGMENTATION_TVM_NODES_EXPORTS) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __declspec(dllexport) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL -#else -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __declspec(dllimport) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL -#endif -#elif defined(__linux__) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __attribute__((visibility("default"))) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __attribute__((visibility("default"))) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // LIDAR_APOLLO_SEGMENTATION_TVM_NODES__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py deleted file mode 100644 index 2d070f2611ac5..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py +++ /dev/null @@ -1,57 +0,0 @@ -# Copyright 2021-2023 Arm Ltd., the Autoware Foundation -# -# 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. - -"""Launch lidar segmentation node.""" - -import os - -from ament_index_python.packages import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -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", - ) - - 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. - lidar_apollo_segmentation_tvm_node_runner = Node( - package="lidar_apollo_segmentation_tvm_nodes", - executable="lidar_apollo_segmentation_tvm_nodes_exe", - remappings=[ - ("points_in", LaunchConfiguration("input/pointcloud")), - ("objects_out", LaunchConfiguration("output/objects")), - ], - parameters=[lidar_apollo_segmentation_tvm_node_params], - output="screen", - ) - - return launch.LaunchDescription(arguments + [lidar_apollo_segmentation_tvm_node_runner]) diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml deleted file mode 100644 index c97d7356f5616..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/package.xml b/perception/lidar_apollo_segmentation_tvm_nodes/package.xml deleted file mode 100755 index 7d3ac6fbe5ac7..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - lidar_apollo_segmentation_tvm_nodes - 0.1.0 - lidar_apollo_segmentation_tvm_nodes - Ambroise Vincent - Yoshi Ri - Apache 2.0 - - ament_cmake - ament_cmake_auto - autoware_cmake - - lidar_apollo_segmentation_tvm - rclcpp - rclcpp_components - sensor_msgs - - ament_lint_auto - autoware_lint_common - ros_testing - - - ament_cmake - - diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json b/perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json deleted file mode 100644 index aaabbf2b65879..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json +++ /dev/null @@ -1,98 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for Lidar Apollo Segmentation TVM Nodes", - "type": "object", - "definitions": { - "lidar_apollo_segmentation_tvm_nodes": { - "type": "object", - "properties": { - "range": { - "type": "integer", - "default": 90, - "exclusiveMinimum": 0, - "description": "The range of the 2D grid with respect to the origin." - }, - "score_threshold": { - "type": "number", - "default": 0.1, - "minimum": 0.0, - "maximum": 1.0, - "description": "The detection confidence score threshold for filtering out the candidate clusters in the post-processing step." - }, - "use_intensity_feature": { - "type": "boolean", - "default": "false", - "description": "Enable input channel intensity feature." - }, - "use_constant_feature": { - "type": "boolean", - "default": "false", - "description": "Enable input channel constant feature." - }, - "z_offset": { - "type": "number", - "default": 0.0, - "description": "Vertical translation of the pointcloud before inference." - }, - "min_height": { - "type": "number", - "default": -5.0, - "description": "The minimum height with respect to the origin" - }, - "max_height": { - "type": "number", - "default": 5.0, - "description": "The maximum height with respect to the origin." - }, - "objectness_thresh": { - "type": "number", - "default": 0.5, - "minimum": 0.0, - "maximum": 1.0, - "description": "The threshold of objectness for filtering out non-object cells in the obstacle clustering step." - }, - "min_pts_num": { - "type": "integer", - "default": 3, - "minimum": 0, - "description": "In the post-processing step, the candidate clusters with less than min_pts_num points are removed." - }, - "height_thresh": { - "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": [ - "range", - "score_threshold", - "use_intensity_feature", - "use_constant_feature", - "z_offset", - "min_height", - "max_height", - "objectness_thresh", - "min_pts_num", - "height_thresh", - "data_path" - ] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/lidar_apollo_segmentation_tvm_nodes" - } - }, - "required": ["ros__parameters"] - } - }, - "required": ["/**"] -} diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp b/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp deleted file mode 100644 index 618d2e258c99b..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2020-2023 Arm Ltd., TierIV -// -// 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 -#include -#include -#include - -#include - -using autoware::perception::lidar_apollo_segmentation_tvm::ApolloLidarSegmentation; - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm_nodes -{ - -ApolloLidarSegmentationNode::ApolloLidarSegmentationNode(const rclcpp::NodeOptions & options) -: Node("lidar_apollo_segmentation_tvm", options), - m_cloud_sub_ptr{create_subscription( - "points_in", rclcpp::SensorDataQoS().keep_last(1), - [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { pointCloudCallback(msg); })}, - m_detected_pub_ptr{create_publisher( - "objects_out", rclcpp::QoS{1})}, - m_detector_ptr{std::make_shared( - declare_parameter("range"), declare_parameter("score_threshold"), - declare_parameter("use_intensity_feature"), - declare_parameter("use_constant_feature"), declare_parameter("z_offset"), - declare_parameter("min_height"), declare_parameter("max_height"), - declare_parameter("objectness_thresh"), declare_parameter("min_pts_num"), - declare_parameter("height_thresh"), declare_parameter("data_path"))} -{ - // Log unexpected versions of the neural network. - auto version_status = m_detector_ptr->version_check(); - if (version_status != tvm_utility::Version::OK) { - auto network_name = m_detector_ptr->network_name(); - if (version_status == tvm_utility::Version::Unknown) { - RCLCPP_INFO( - get_logger(), "The '%s' network doesn't provide a version number.", network_name.c_str()); - } else if (version_status == tvm_utility::Version::Untested) { - RCLCPP_WARN( - get_logger(), "The version of the '%s' network is untested.", network_name.c_str()); - } else if (version_status == tvm_utility::Version::Unsupported) { - RCLCPP_ERROR( - get_logger(), "The version of the '%s' network is unsupported.", network_name.c_str()); - } - } -} - -void ApolloLidarSegmentationNode::pointCloudCallback( - const sensor_msgs::msg::PointCloud2::SharedPtr & msg) -{ - std::shared_ptr output_msg; - try { - output_msg = m_detector_ptr->detectDynamicObjects(*msg); - } catch (const std::exception & e) { - RCLCPP_WARN(get_logger(), e.what()); - return; - } - m_detected_pub_ptr->publish(*output_msg); -} -} // namespace lidar_apollo_segmentation_tvm_nodes -} // namespace perception -} // namespace autoware - -RCLCPP_COMPONENTS_REGISTER_NODE( - autoware::perception::lidar_apollo_segmentation_tvm_nodes::ApolloLidarSegmentationNode) diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py b/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py deleted file mode 100644 index 0a9adafc3f641..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py +++ /dev/null @@ -1,60 +0,0 @@ -# Copyright 2021-2022 Arm Ltd. -# -# 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. - -import os -import unittest - -from ament_index_python import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node -import launch_testing -import pytest - - -@pytest.mark.launch_test -def generate_test_description(): - lidar_apollo_segmentation_tvm = Node( - package="lidar_apollo_segmentation_tvm_nodes", - executable="lidar_apollo_segmentation_tvm_nodes_exe", - name="lidar_apollo_segmentation_tvm_nodes", - namespace="benchmark", - output="screen", - parameters=[ - os.path.join( - get_package_share_directory("lidar_apollo_segmentation_tvm_nodes"), - "param/test.param.yaml", - ) - ], - ) - - context = {"lidar_apollo_segmentation_tvm": lidar_apollo_segmentation_tvm} - - launch_description = LaunchDescription( - [ - lidar_apollo_segmentation_tvm, - # Start tests right away - no need to wait for anything - launch_testing.actions.ReadyToTest(), - ] - ) - - return launch_description, context - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_output, proc_info, lidar_apollo_segmentation_tvm): - # Check that process exits with code -2 or -15 - launch_testing.asserts.assertExitCodes( - proc_info, [-2, -15], process=lidar_apollo_segmentation_tvm - )