diff --git a/sensing/laserscan_to_occupancy_grid_map/CMakeLists.txt b/sensing/laserscan_to_occupancy_grid_map/CMakeLists.txt new file mode 100644 index 0000000000000..05fb920933356 --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5) +project(laserscan_to_occupancy_grid_map) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(laserscan_to_occupancy_grid_map SHARED + src/laserscan_to_occupancy_grid_map_node.cpp + src/occupancy_grid_map.cpp + src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +) + +target_link_libraries(laserscan_to_occupancy_grid_map + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(laserscan_to_occupancy_grid_map + PLUGIN "occupancy_grid_map::OccupancyGridMapNode" + EXECUTABLE laserscan_to_occupancy_grid_map_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE + # To avoid conflicts between cpplint and uncrustify w.r.t. inclusion guards + ament_cmake_uncrustify + ) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/sensing/laserscan_to_occupancy_grid_map/README.md b/sensing/laserscan_to_occupancy_grid_map/README.md new file mode 100644 index 0000000000000..6d3417409613d --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/README.md @@ -0,0 +1,82 @@ +# laserscan_to_occupancy_grid_map + +## Purpose + +This package outputs the probability of having an obstacle as occupancy grid map. + +![occupancy_grid_map_sample_image](./image/occupancy_grid_map_sample_image.png) + +## Inner-workings / Algorithms + +The basic idea is to take a 2D laserscan and ray trace it to create a time-series processed occupancy grid map. + +1. the node take a laserscan and make an occupancy grid map with one frame. ray trace is done by Bresenham's line algorithm. + ![Bresenham's line algorithm](./image/bresenham.svg) +2. Optionally, obstacle point clouds and raw point clouds can be received and reflected in the occupancy grid map. The reason is that laserscan only uses the most foreground point in the polar coordinate system, so it throws away a lot of information. As a result, the occupancy grid map is almost an UNKNOWN cell. + Therefore, the obstacle point cloud and the raw point cloud are used to reflect what is judged to be the ground and what is judged to be an obstacle in the occupancy grid map. + ![Bresenham's line algorithm](./image/update_with_pointcloud.svg) + The black and red dots represent raw point clouds, and the red dots represent obstacle point clouds. In other words, the black points are determined as the ground, and the red point cloud is the points determined as obstacles. The gray cells are represented as UNKNOWN cells. + +3. Using the previous occupancy grid map, update the existence probability using a binary Bayesian filter (1). Also, the unobserved cells are time-decayed like the system noise of the Kalman filter (2). + +```math + \hat{P_{o}} = \frac{(P_{o} * P_{z})}{(P_{o} * P_{z} + (1 - P_{o}) * \bar{P_{z}})} \tag{1} +``` + +```math + \hat{P_{o}} = \frac{(P_{o} + 0.5 * \frac{1}{ratio})}{(\frac{1}{ratio} + 1)} \tag{2} +``` + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ----------------------------- | -------------------------- | -------------------------------------------------------------- | +| `~/input/laserscan` | `sensor_msgs::LaserScan` | laserscan | +| `~/input/obstacle_pointcloud` | `sensor_msgs::PointCloud2` | obstacle pointcloud | +| `~/input/raw_pointcloud` | `sensor_msgs::PointCloud2` | The overall point cloud used to input the obstacle point cloud | + +### Output + +| Name | Type | Description | +| ----------------------------- | ------------------------- | ------------------ | +| `~/output/occupancy_grid_map` | `nav_msgs::OccupancyGrid` | occupancy grid map | + +## Parameters + +### Node Parameters + +| Name | Type | Description | +| ----------------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `map_frame` | string | map frame | +| `base_link_frame` | string | base_link frame | +| `input_obstacle_pointcloud` | bool | whether to use the optional obstacle point cloud? If this is true, `~/input/obstacle_pointcloud` topics will be received. | +| `input_obstacle_and_raw_pointcloud` | bool | whether to use the optional obstacle and raw point cloud? If this is true, `~/input/obstacle_pointcloud` and `~/input/raw_pointcloud` topics will be received. | +| `use_height_filter` | bool | whether to height filter for `~/input/obstacle_pointcloud` and `~/input/raw_pointcloud`? By default, the height is set to -1~2m. | +| `map_length` | double | The length of the map. -100 if it is 50~50[m] | +| `map_resolution` | double | The map cell resolution [m] | + +## Assumptions / Known limits + +In several places we have modified the external code written in BSD3 license. + +- occupancy_grid_map.hpp +- cost_value.hpp +- occupancy_grid_map.cpp + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +Bresenham's_line_algorithm + +- +- + +## (Optional) Future extensions / Unimplemented parts + +- The update probability of the binary Bayesian filter is currently hard-coded and requires a code change to be modified. +- Since there is no special support for moving objects, the probability of existence is not increased for fast objects. diff --git a/sensing/laserscan_to_occupancy_grid_map/image/bresenham.svg b/sensing/laserscan_to_occupancy_grid_map/image/bresenham.svg new file mode 100644 index 0000000000000..61b107efe3f9e --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/image/bresenham.svg @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/sensing/laserscan_to_occupancy_grid_map/image/occupancy_grid_map_sample_image.png b/sensing/laserscan_to_occupancy_grid_map/image/occupancy_grid_map_sample_image.png new file mode 100644 index 0000000000000..ee9881071fb34 Binary files /dev/null and b/sensing/laserscan_to_occupancy_grid_map/image/occupancy_grid_map_sample_image.png differ diff --git a/sensing/laserscan_to_occupancy_grid_map/image/update_with_pointcloud.svg b/sensing/laserscan_to_occupancy_grid_map/image/update_with_pointcloud.svg new file mode 100644 index 0000000000000..ebdaf0351b0ca --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/image/update_with_pointcloud.svg @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/cost_value.hpp b/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/cost_value.hpp new file mode 100644 index 0000000000000..f5edd9a0a6fe7 --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/cost_value.hpp @@ -0,0 +1,77 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + *********************************************************************/ + +#ifndef LASERSCAN_TO_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ + +#include + +namespace occupancy_cost_value +{ +static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 +static const unsigned char LETHAL_OBSTACLE = 255; +static const unsigned char FREE_SPACE = 0; + +struct CostTranslationTable +{ + CostTranslationTable() + { + for (int i = 0; i < 256; i++) { + const auto value = static_cast(static_cast(i) * 100.f / 255.f); + data[i] = std::max(std::min(value, static_cast(99)), static_cast(1)); + } + } + char operator[](unsigned char n) const { return data[n]; } + char data[256]; +}; +static const CostTranslationTable cost_translation_table; +} // namespace occupancy_cost_value + +#endif // LASERSCAN_TO_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ diff --git a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/laserscan_to_occupancy_grid_map_node.hpp b/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/laserscan_to_occupancy_grid_map_node.hpp new file mode 100644 index 0000000000000..670b7404b0e6e --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/laserscan_to_occupancy_grid_map_node.hpp @@ -0,0 +1,100 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 LASERSCAN_TO_OCCUPANCY_GRID_MAP__LASERSCAN_TO_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__LASERSCAN_TO_OCCUPANCY_GRID_MAP_NODE_HPP_ + +#include "laserscan_to_occupancy_grid_map/occupancy_grid_map.hpp" +#include "laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace occupancy_grid_map +{ +using builtin_interfaces::msg::Time; +using costmap_2d::OccupancyGridMapUpdaterInterface; +using laser_geometry::LaserProjection; +using nav2_costmap_2d::Costmap2D; +using nav_msgs::msg::OccupancyGrid; +using sensor_msgs::msg::LaserScan; +using sensor_msgs::msg::PointCloud2; +using tf2_ros::Buffer; +using tf2_ros::TransformListener; + +class OccupancyGridMapNode : public rclcpp::Node +{ +public: + explicit OccupancyGridMapNode(const rclcpp::NodeOptions & node_options); + +private: + PointCloud2::SharedPtr convertLaserscanToPointCLoud2(const LaserScan::ConstSharedPtr & input); + void onLaserscanPointCloud2WithObstacleAndRaw( + const LaserScan::ConstSharedPtr & input_laserscan_msg, + const PointCloud2::ConstSharedPtr & input_obstacle_msg, + const PointCloud2::ConstSharedPtr & input_raw_msg); + OccupancyGrid::UniquePtr OccupancyGridMapToMsgPtr( + const std::string & frame_id, const Time & stamp, const float & robot_pose_z, + const Costmap2D & occupancy_grid_map); + inline void onDummyPointCloud2(const LaserScan::ConstSharedPtr & input) + { + PointCloud2 dummy; + sensor_msgs::PointCloud2Modifier modifier(dummy); + modifier.setPointCloud2FieldsByString(1, "xyz"); + dummy.header = input->header; + passthrough_.add(std::make_shared(dummy)); + } + +private: + rclcpp::Publisher::SharedPtr occupancy_grid_map_pub_; + message_filters::Subscriber laserscan_sub_; + message_filters::Subscriber obstacle_pointcloud_sub_; + message_filters::Subscriber raw_pointcloud_sub_; + message_filters::PassThrough passthrough_; + + std::shared_ptr tf2_{std::make_shared(get_clock())}; + std::shared_ptr tf2_listener_{std::make_shared(*tf2_)}; + + using SyncPolicy = message_filters::sync_policies::ExactTime; + using Sync = message_filters::Synchronizer; + std::shared_ptr sync_ptr_; + + LaserProjection laserscan2pointcloud_converter_; + + std::shared_ptr occupancy_grid_map_updater_ptr_; + + // ROS Parameters + std::string map_frame_; + std::string base_link_frame_; + bool use_height_filter_; +}; + +} // namespace occupancy_grid_map + +#endif // LASERSCAN_TO_OCCUPANCY_GRID_MAP__LASERSCAN_TO_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/occupancy_grid_map.hpp b/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/occupancy_grid_map.hpp new file mode 100644 index 0000000000000..7ecb2ce8a9faa --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/occupancy_grid_map.hpp @@ -0,0 +1,94 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +#ifndef LASERSCAN_TO_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ + +#include +#include + +#include +#include +#include + +namespace costmap_2d +{ +using geometry_msgs::msg::Pose; +using sensor_msgs::msg::PointCloud2; + +class OccupancyGridMap : public nav2_costmap_2d::Costmap2D +{ +public: + OccupancyGridMap( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution); + + void raytrace2D(const PointCloud2 & pointcloud, const Pose & robot_pose); + + void updateFreespaceCells(const PointCloud2 & pointcloud); + + void updateOccupiedCells(const PointCloud2 & pointcloud); + + void updateOrigin(double new_origin_x, double new_origin_y) override; + +private: + void raytraceFreespace(const PointCloud2 & pointcloud, const Pose & robot_pose); + + void updateCellsByPointCloud(const PointCloud2 & pointcloud, const unsigned char cost); + + bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; + + rclcpp::Logger logger_{rclcpp::get_logger("occupancy_grid_map")}; + rclcpp::Clock clock_{RCL_ROS_TIME}; +}; + +} // namespace costmap_2d + +#endif // LASERSCAN_TO_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ diff --git a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp new file mode 100644 index 0000000000000..2e351df7b6a78 --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp @@ -0,0 +1,50 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +// *INDENT-OFF* +#ifndef LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +// *INDENT-ON* + +#include "laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" + +#include +#include + +namespace costmap_2d +{ +class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface +{ +public: + enum Index : size_t { OCCUPIED = 0U, FREE = 1U }; + OccupancyGridMapBBFUpdater( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) + : OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution) + { + probability_matrix_(Index::OCCUPIED, Index::OCCUPIED) = 0.95; + probability_matrix_(Index::FREE, Index::OCCUPIED) = + 1.0 - probability_matrix_(OCCUPIED, OCCUPIED); + probability_matrix_(Index::FREE, Index::FREE) = 0.8; + probability_matrix_(Index::OCCUPIED, Index::FREE) = 1.0 - probability_matrix_(FREE, FREE); + } + bool update(const Costmap2D & oneshot_occupancy_grid_map) override; + +private: + inline unsigned char applyBBF(const unsigned char & z, const unsigned char & o); + Eigen::Matrix2f probability_matrix_; +}; + +} // namespace costmap_2d + +#endif // LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp b/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp new file mode 100644 index 0000000000000..33615fc9065e5 --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/include/laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp @@ -0,0 +1,39 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#define LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ + +#include "laserscan_to_occupancy_grid_map/cost_value.hpp" + +#include + +namespace costmap_2d +{ +class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D +{ +public: + OccupancyGridMapUpdaterInterface( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) + : Costmap2D( + cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) + { + } + virtual ~OccupancyGridMapUpdaterInterface() = default; + virtual bool update(const Costmap2D & oneshot_occupancy_grid_map) = 0; +}; + +} // namespace costmap_2d + +#endif // LASERSCAN_TO_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ diff --git a/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py b/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py new file mode 100644 index 0000000000000..fc7b9323ad183 --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py @@ -0,0 +1,139 @@ +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + composable_nodes = [ + ComposableNode( + package="pointcloud_to_laserscan", + plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode", + name="pointcloud_to_laserscan_node", + remappings=[ + ("~/input/pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), + ("~/output/laserscan", LaunchConfiguration("output/laserscan")), + ("~/output/pointcloud", LaunchConfiguration("output/pointcloud")), + ("~/output/ray", LaunchConfiguration("output/ray")), + ("~/output/stixel", LaunchConfiguration("output/stixel")), + ], + parameters=[ + { + "target_frame": "base_link", # Leave disabled to output scan in pointcloud frame + "transform_tolerance": 0.01, + "min_height": 0.0, + "max_height": 2.0, + "angle_min": -3.141592, # -M_PI + "angle_max": 3.141592, # M_PI + "angle_increment": 0.00436332222, # 0.25*M_PI/180.0 + "scan_time": 0.0, + "range_min": 1.0, + "range_max": 200.0, + "use_inf": True, + "inf_epsilon": 1.0, + # Concurrency level, affects number of pointclouds queued for processing + # and number of threads used + # 0 : Detect number of cores + # 1 : Single threaded + # 2->inf : Parallelism level + "concurrency_level": 1, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ), + ComposableNode( + package="laserscan_to_occupancy_grid_map", + plugin="occupancy_grid_map::OccupancyGridMapNode", + name="occupancy_grid_map_node", + remappings=[ + ("~/input/laserscan", LaunchConfiguration("output/laserscan")), + ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), + ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), + ("~/output/occupancy_grid_map", LaunchConfiguration("output")), + ], + parameters=[ + { + "map_resolution": 0.5, + "use_height_filter": True, + "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"), + "input_obstacle_and_raw_pointcloud": LaunchConfiguration( + "input_obstacle_and_raw_pointcloud" + ), + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ), + ] + + occupancy_grid_map_container = ComposableNodeContainer( + condition=LaunchConfigurationEquals("container", ""), + name="occupancy_grid_map_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=composable_nodes, + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + condition=LaunchConfigurationNotEquals("container", ""), + composable_node_descriptions=composable_nodes, + target_container=LaunchConfiguration("container"), + ) + + return LaunchDescription( + [ + add_launch_arg("container", ""), + add_launch_arg("use_multithread", "false"), + add_launch_arg("use_intra_process", "false"), + add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), + add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), + add_launch_arg("output", "occupancy_grid"), + add_launch_arg("output/laserscan", "virtual_scan/laserscan"), + add_launch_arg("output/pointcloud", "virtual_scan/pointcloud"), + add_launch_arg("output/ray", "virtual_scan/ray"), + add_launch_arg("output/stixel", "virtual_scan/stixel"), + add_launch_arg("input_obstacle_pointcloud", "false"), + add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), + set_container_executable, + set_container_mt_executable, + occupancy_grid_map_container, + load_composable_nodes, + ] + ) diff --git a/sensing/laserscan_to_occupancy_grid_map/package.xml b/sensing/laserscan_to_occupancy_grid_map/package.xml new file mode 100644 index 0000000000000..503b3a4ecc304 --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/package.xml @@ -0,0 +1,36 @@ + + + laserscan_to_occupancy_grid_map + 0.1.0 + generate occupancy grid map from laser scan + + Yukihiro Saito + Yukihiro Saito + Apache License 2.0 + ament_cmake_auto + + autoware_utils + eigen3_cmake_module + laser_geometry + message_filters + nav2_costmap_2d + nav_msgs + pcl_ros + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + visualization_msgs + + pointcloud_to_laserscan + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp b/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp new file mode 100644 index 0000000000000..50e5784abbce0 --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp @@ -0,0 +1,277 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "laserscan_to_occupancy_grid_map/laserscan_to_occupancy_grid_map_node.hpp" + +#include "laserscan_to_occupancy_grid_map/cost_value.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +namespace +{ +bool transformPointcloud( + const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2, + const std::string & target_frame, sensor_msgs::msg::PointCloud2 & output) +{ + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped = tf2.lookupTransform( + target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + // transform pointcloud + Eigen::Matrix4f tf_matrix = tf2::transformToEigen(tf_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(tf_matrix, input, output); + output.header.stamp = input.header.stamp; + output.header.frame_id = target_frame; + return true; +} + +bool cropPointcloudByHeight( + const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2, + const std::string & target_frame, const float min_height, const float max_height, + sensor_msgs::msg::PointCloud2 & output) +{ + rclcpp::Clock clock{RCL_ROS_TIME}; + // Transformed pointcloud on target frame + sensor_msgs::msg::PointCloud2 trans_input_tmp; + const bool is_target_frame = (input.header.frame_id == target_frame); + if (!is_target_frame) { + try { + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped = tf2.lookupTransform( + target_frame, input.header.frame_id, input.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + // transform pointcloud + Eigen::Matrix4f tf_matrix = + tf2::transformToEigen(tf_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(tf_matrix, input, trans_input_tmp); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("laserscan_to_occupancy_grid_map"), clock, 5000, "%s", ex.what()); + return false; + } + } + const sensor_msgs::msg::PointCloud2 & trans_input = is_target_frame ? input : trans_input_tmp; + + // Apply height filter + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + for (sensor_msgs::PointCloud2ConstIterator iter_x(trans_input, "x"), + iter_y(trans_input, "y"), iter_z(trans_input, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + if (min_height < *iter_z && *iter_z < max_height) { + pcl_output->push_back(pcl::PointXYZ(*iter_x, *iter_y, *iter_z)); + } + } + + // Convert to ros msg + pcl::toROSMsg(*pcl_output, output); + output.header = input.header; + return true; +} + +geometry_msgs::msg::Pose getPose( + const std_msgs::msg::Header & source_header, const tf2_ros::Buffer & tf2, + const std::string & target_frame) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped = tf2.lookupTransform( + target_frame, source_header.frame_id, source_header.stamp, rclcpp::Duration::from_seconds(0.5)); + pose = autoware_utils::transform2pose(tf_stamped.transform); + return pose; +} +} // namespace + +namespace occupancy_grid_map +{ +using costmap_2d::OccupancyGridMap; +using costmap_2d::OccupancyGridMapBBFUpdater; +using geometry_msgs::msg::Pose; + +OccupancyGridMapNode::OccupancyGridMapNode(const rclcpp::NodeOptions & node_options) +: Node("occupancy_grid_map_node", node_options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + + /* params */ + map_frame_ = declare_parameter("map_frame", "map"); + base_link_frame_ = declare_parameter("base_link_frame", "base_link"); + use_height_filter_ = declare_parameter("use_height_filter", true); + const double map_length{declare_parameter("map_length", 100.0)}; + const double map_resolution{declare_parameter("map_resolution", 0.5)}; + const bool input_obstacle_pointcloud{declare_parameter("input_obstacle_pointcloud", true)}; + const bool input_obstacle_and_raw_pointcloud{ + declare_parameter("input_obstacle_and_raw_pointcloud", true)}; + + /* Subscriber and publisher */ + laserscan_sub_.subscribe( + this, "~/input/laserscan", rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + obstacle_pointcloud_sub_.subscribe( + this, "~/input/obstacle_pointcloud", + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + raw_pointcloud_sub_.subscribe( + this, "~/input/raw_pointcloud", rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + // add dummy callback to enable passthrough filter + laserscan_sub_.registerCallback(std::bind(&OccupancyGridMapNode::onDummyPointCloud2, this, _1)); + if (input_obstacle_and_raw_pointcloud) { + sync_ptr_ = std::make_shared( + SyncPolicy(5), laserscan_sub_, obstacle_pointcloud_sub_, raw_pointcloud_sub_); + } else if (input_obstacle_pointcloud) { + sync_ptr_ = + std::make_shared(SyncPolicy(3), laserscan_sub_, obstacle_pointcloud_sub_, passthrough_); + } else { + sync_ptr_ = std::make_shared(SyncPolicy(3), laserscan_sub_, passthrough_, passthrough_); + } + + sync_ptr_->registerCallback( + std::bind(&OccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw, this, _1, _2, _3)); + occupancy_grid_map_pub_ = create_publisher("~/output/occupancy_grid_map", 1); + + /* Occupancy grid */ + occupancy_grid_map_updater_ptr_ = std::make_shared( + map_length / map_resolution, map_length / map_resolution, map_resolution); +} + +PointCloud2::SharedPtr OccupancyGridMapNode::convertLaserscanToPointCLoud2( + const LaserScan::ConstSharedPtr & input) +{ + // check over max range point + const float max_range = + static_cast(occupancy_grid_map_updater_ptr_->getSizeInCellsX()) * 0.5f + + occupancy_grid_map_updater_ptr_->getResolution(); + constexpr float epsilon = 0.001; + LaserScan laserscan = *input; + laserscan.range_max = max_range; + for (auto & range : laserscan.ranges) { + if (max_range < range || std::isinf(range)) { + range = max_range - epsilon; + } + } + + // convert to pointcloud + PointCloud2::SharedPtr pointcloud_ptr = std::make_shared(); + pointcloud_ptr->header = laserscan.header; + laserscan2pointcloud_converter_.transformLaserScanToPointCloud( + laserscan.header.frame_id, laserscan, *pointcloud_ptr, *tf2_); + + return pointcloud_ptr; +} + +void OccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw( + const LaserScan::ConstSharedPtr & input_laserscan_msg, + const PointCloud2::ConstSharedPtr & input_obstacle_msg, + const PointCloud2::ConstSharedPtr & input_raw_msg) +{ + // Laserscan to pointcloud2 + PointCloud2::ConstSharedPtr laserscan_pc_ptr = convertLaserscanToPointCLoud2(input_laserscan_msg); + + // Apply height filter + PointCloud2 cropped_obstacle_pc{}; + PointCloud2 cropped_raw_pc{}; + if (use_height_filter_) { + constexpr float min_height = -1.0, max_height = 2.0; + if (!cropPointcloudByHeight( + *input_obstacle_msg, *tf2_, base_link_frame_, min_height, max_height, + cropped_obstacle_pc)) { + return; + } + if (!cropPointcloudByHeight( + *input_raw_msg, *tf2_, base_link_frame_, min_height, max_height, cropped_raw_pc)) { + return; + } + } + const PointCloud2 & filtered_obstacle_pc = + use_height_filter_ ? cropped_obstacle_pc : *input_obstacle_msg; + const PointCloud2 & filtered_raw_pc = use_height_filter_ ? cropped_raw_pc : *input_raw_msg; + + // Transform pointcloud and get frame pose + PointCloud2 trans_laserscan_pc{}; + PointCloud2 trans_obstacle_pc{}; + PointCloud2 trans_raw_pc{}; + Pose pose{}; + try { + transformPointcloud(*laserscan_pc_ptr, *tf2_, map_frame_, trans_laserscan_pc); + transformPointcloud(filtered_obstacle_pc, *tf2_, map_frame_, trans_obstacle_pc); + transformPointcloud(filtered_raw_pc, *tf2_, map_frame_, trans_raw_pc); + pose = getPose(laserscan_pc_ptr->header, *tf2_, map_frame_); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(get_logger(), ex.what()); + return; + } + + // Create oneshot occupancy grid map + OccupancyGridMap oneshot_occupancy_grid_map( + occupancy_grid_map_updater_ptr_->getSizeInCellsX(), + occupancy_grid_map_updater_ptr_->getSizeInCellsY(), + occupancy_grid_map_updater_ptr_->getResolution()); + oneshot_occupancy_grid_map.updateOrigin( + pose.position.x - oneshot_occupancy_grid_map.getSizeInMetersX() / 2, + pose.position.y - oneshot_occupancy_grid_map.getSizeInMetersY() / 2); + oneshot_occupancy_grid_map.updateFreespaceCells(trans_raw_pc); + oneshot_occupancy_grid_map.raytrace2D(trans_laserscan_pc, pose); + oneshot_occupancy_grid_map.updateOccupiedCells(trans_obstacle_pc); + + // Update with bayes filter + occupancy_grid_map_updater_ptr_->update(oneshot_occupancy_grid_map); + + // publish + occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr( + map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z, *occupancy_grid_map_updater_ptr_)); +} + +OccupancyGrid::UniquePtr OccupancyGridMapNode::OccupancyGridMapToMsgPtr( + const std::string & frame_id, const Time & stamp, const float & robot_pose_z, + const Costmap2D & occupancy_grid_map) +{ + auto msg_ptr = std::make_unique(); + + msg_ptr->header.frame_id = frame_id; + msg_ptr->header.stamp = stamp; + msg_ptr->info.resolution = occupancy_grid_map.getResolution(); + + msg_ptr->info.width = occupancy_grid_map.getSizeInCellsX(); + msg_ptr->info.height = occupancy_grid_map.getSizeInCellsY(); + + double wx{}; + double wy{}; + occupancy_grid_map.mapToWorld(0, 0, wx, wy); + msg_ptr->info.origin.position.x = occupancy_grid_map.getOriginX(); + msg_ptr->info.origin.position.y = occupancy_grid_map.getOriginY(); + msg_ptr->info.origin.position.z = robot_pose_z; + msg_ptr->info.origin.orientation.w = 1.0; + + msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); + + unsigned char * data = occupancy_grid_map.getCharMap(); + for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { + msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + } + return msg_ptr; +} + +} // namespace occupancy_grid_map + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::OccupancyGridMapNode) diff --git a/sensing/laserscan_to_occupancy_grid_map/src/occupancy_grid_map.cpp b/sensing/laserscan_to_occupancy_grid_map/src/occupancy_grid_map.cpp new file mode 100644 index 0000000000000..24b0c65adbb81 --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/src/occupancy_grid_map.cpp @@ -0,0 +1,252 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +#include "laserscan_to_occupancy_grid_map/occupancy_grid_map.hpp" + +#include "laserscan_to_occupancy_grid_map/cost_value.hpp" + +#include + +#include + +namespace costmap_2d +{ +using sensor_msgs::PointCloud2ConstIterator; + +OccupancyGridMap::OccupancyGridMap( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) +{ +} + +bool OccupancyGridMap::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const +{ + if (wx < origin_x_ || wy < origin_y_) { + return false; + } + + mx = static_cast(std::floor((wx - origin_x_) / resolution_)); + my = static_cast(std::floor((wy - origin_y_) / resolution_)); + + if (mx < size_x_ && my < size_y_) { + return true; + } + + return false; +} + +void OccupancyGridMap::updateOrigin(double new_origin_x, double new_origin_y) +{ + // project the new origin into the grid + int cell_ox{static_cast(std::floor((new_origin_x - origin_x_) / resolution_))}; + int cell_oy{static_cast(std::floor((new_origin_y - origin_y_) / resolution_))}; + + // compute the associated world coordinates for the origin cell + // because we want to keep things grid-aligned + double new_grid_ox{origin_x_ + cell_ox * resolution_}; + double new_grid_oy{origin_y_ + cell_oy * resolution_}; + + // To save casting from unsigned int to int a bunch of times + int size_x{static_cast(size_x_)}; + int size_y{static_cast(size_y_)}; + + // we need to compute the overlap of the new and existing windows + int lower_left_x{std::min(std::max(cell_ox, 0), size_x)}; + int lower_left_y{std::min(std::max(cell_oy, 0), size_y)}; + int upper_right_x{std::min(std::max(cell_ox + size_x, 0), size_x)}; + int upper_right_y{std::min(std::max(cell_oy + size_y, 0), size_y)}; + + unsigned int cell_size_x = upper_right_x - lower_left_x; + unsigned int cell_size_y = upper_right_y - lower_left_y; + + // we need a map to store the obstacles in the window temporarily + unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y]; + + // copy the local window in the costmap to the local map + copyMapRegion( + costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, + cell_size_y); + + // now we'll set the costmap to be completely unknown if we track unknown space + resetMaps(); + + // update the origin with the appropriate world coordinates + origin_x_ = new_grid_ox; + origin_y_ = new_grid_oy; + + // compute the starting cell location for copying data back in + int start_x{lower_left_x - cell_ox}; + int start_y{lower_left_y - cell_oy}; + + // now we want to copy the overlapping information back into the map, but in its new location + copyMapRegion( + local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); + + // make sure to clean up + delete[] local_map; +} + +void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & robot_pose) +{ + // freespace + raytraceFreespace(pointcloud, robot_pose); + + // occupied + MarkCell marker(costmap_, occupancy_cost_value::LETHAL_OBSTACLE); + for (PointCloud2ConstIterator iter_x(pointcloud, "x"), iter_y(pointcloud, "y"); + iter_x != iter_x.end(); ++iter_x, ++iter_y) { + unsigned int mx, my; + if (!worldToMap(*iter_x, *iter_y, mx, my)) { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + const unsigned int index = getIndex(mx, my); + marker(index); + } +} + +void OccupancyGridMap::updateFreespaceCells(const PointCloud2 & pointcloud) +{ + updateCellsByPointCloud(pointcloud, occupancy_cost_value::FREE_SPACE); +} + +void OccupancyGridMap::updateOccupiedCells(const PointCloud2 & pointcloud) +{ + updateCellsByPointCloud(pointcloud, occupancy_cost_value::LETHAL_OBSTACLE); +} + +void OccupancyGridMap::updateCellsByPointCloud( + const PointCloud2 & pointcloud, const unsigned char cost) +{ + MarkCell marker(costmap_, cost); + for (PointCloud2ConstIterator iter_x(pointcloud, "x"), iter_y(pointcloud, "y"); + iter_x != iter_x.end(); ++iter_x, ++iter_y) { + unsigned int mx{}; + unsigned int my{}; + if (!worldToMap(*iter_x, *iter_y, mx, my)) { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + const unsigned int index = getIndex(mx, my); + marker(index); + } +} + +void OccupancyGridMap::raytraceFreespace(const PointCloud2 & pointcloud, const Pose & robot_pose) +{ + unsigned int x0{}; + unsigned int y0{}; + const double ox{robot_pose.position.x}; + const double oy{robot_pose.position.y}; + if (!worldToMap(robot_pose.position.x, robot_pose.position.y, x0, y0)) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, + "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot " + "raytrace for it.", + ox, oy); + return; + } + + // we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later + const double origin_x = origin_x_, origin_y = origin_y_; + const double map_end_x = origin_x + size_x_ * resolution_; + const double map_end_y = origin_y + size_y_ * resolution_; + + for (PointCloud2ConstIterator iter_x(pointcloud, "x"), iter_y(pointcloud, "y"); + iter_x != iter_x.end(); ++iter_x, ++iter_y) { + double wx = *iter_x; + double wy = *iter_y; + + // now we also need to make sure that the endpoint we're ray-tracing + // to isn't off the costmap and scale if necessary + const double a = wx - ox; + const double b = wy - oy; + + // the minimum value to raytrace from is the origin + if (wx < origin_x) { + const double t = (origin_x - ox) / a; + wx = origin_x; + wy = oy + b * t; + } + if (wy < origin_y) { + const double t = (origin_y - oy) / b; + wx = ox + a * t; + wy = origin_y; + } + + // the maximum value to raytrace to is the end of the map + if (wx > map_end_x) { + const double t = (map_end_x - ox) / a; + wx = map_end_x - .001; + wy = oy + b * t; + } + if (wy > map_end_y) { + const double t = (map_end_y - oy) / b; + wx = ox + a * t; + wy = map_end_y - .001; + } + + // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint + unsigned int x1{}; + unsigned int y1{}; + + // check for legality just in case + if (!worldToMap(wx, wy, x1, y1)) { + continue; + } + + constexpr unsigned int cell_raytrace_range = 10000; // large number to ignore range threshold + MarkCell marker(costmap_, occupancy_cost_value::FREE_SPACE); + raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); + } +} + +} // namespace costmap_2d diff --git a/sensing/laserscan_to_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/sensing/laserscan_to_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp new file mode 100644 index 0000000000000..100fcd517e11e --- /dev/null +++ b/sensing/laserscan_to_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp @@ -0,0 +1,59 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "laserscan_to_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" + +#include "laserscan_to_occupancy_grid_map/cost_value.hpp" + +#include + +namespace costmap_2d +{ +inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( + const unsigned char & z, const unsigned char & o) +{ + constexpr float cost2p = 1.f / 255.f; + const float po = o * cost2p; + float pz{}; + float not_pz{}; + float po_hat{}; + if (z == occupancy_cost_value::LETHAL_OBSTACLE) { + pz = probability_matrix_(Index::OCCUPIED, Index::OCCUPIED); + not_pz = probability_matrix_(Index::FREE, Index::OCCUPIED); + po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); + } else if (z == occupancy_cost_value::FREE_SPACE) { + pz = 1.f - probability_matrix_(Index::FREE, Index::FREE); + not_pz = 1.f - probability_matrix_(Index::OCCUPIED, Index::FREE); + po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); + } else if (z == occupancy_cost_value::NO_INFORMATION) { + constexpr float inv_v_ratio = 1.f / 10.f; + po_hat = ((po + (0.5f * inv_v_ratio)) / ((1.f * inv_v_ratio) + 1.f)); + } + return std::min( + std::max(static_cast(po_hat * 255.f + 0.5f), static_cast(1)), + static_cast(254)); +} + +bool OccupancyGridMapBBFUpdater::update(const Costmap2D & oneshot_occupancy_grid_map) +{ + updateOrigin(oneshot_occupancy_grid_map.getOriginX(), oneshot_occupancy_grid_map.getOriginY()); + for (unsigned int x = 0; x < getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < getSizeInCellsY(); y++) { + unsigned int index = getIndex(x, y); + costmap_[index] = applyBBF(oneshot_occupancy_grid_map.getCost(x, y), costmap_[index]); + } + } + return true; +} +} // namespace costmap_2d diff --git a/sensing/livox/livox_tag_filter/CMakeLists.txt b/sensing/livox/livox_tag_filter/CMakeLists.txt new file mode 100644 index 0000000000000..bb0cf8166e3b3 --- /dev/null +++ b/sensing/livox/livox_tag_filter/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.5) +project(livox_tag_filter) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(PCL REQUIRED) +include_directories(${PCL_INCLUDE_DIRS}) + +# Target +## livox_tag_filter_node +ament_auto_add_library(livox_tag_filter SHARED + src/livox_tag_filter_node/livox_tag_filter_node.cpp +) + +target_link_libraries(livox_tag_filter + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(livox_tag_filter + PLUGIN "livox_tag_filter::LivoxTagFilterNode" + EXECUTABLE livox_tag_filter_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +# Install +## executables and libraries +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/sensing/livox/livox_tag_filter/README.md b/sensing/livox/livox_tag_filter/README.md new file mode 100644 index 0000000000000..21f62355d49df --- /dev/null +++ b/sensing/livox/livox_tag_filter/README.md @@ -0,0 +1,65 @@ +# livox_tag_filter + +## Purpose + +The `livox_tag_filter` is a node that removes noise from pointcloud by using the following tags: + +- Point property based on spatial position +- Point property based on intensity +- Return number + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------- | ------------------------------- | ---------------- | +| `~/input` | `sensor_msgs::msg::PointCloud2` | reference points | + +### Output + +| Name | Type | Description | +| ---------- | ------------------------------- | --------------- | +| `~/output` | `sensor_msgs::msg::PointCloud2` | filtered points | + +## Parameters + +### Node Parameters + +| Name | Type | Description | +| ------------- | --------------- | -------------------------------------- | +| `ignore_tags` | vector | ignored tags (See the following table) | + +### Tag Parameters + +| Bit | Description | Options | +| --- | ---------------------------------------- | ------------------------------------------ | +| 0~1 | Point property based on spatial position | 00: Normal | +| | | 01: High confidence level of the noise | +| | | 10: Moderate confidence level of the noise | +| | | 11: Low confidence level of the noise | +| 2~3 | Point property based on intensity | 00: Normal | +| | | 01: High confidence level of the noise | +| | | 10: Moderate confidence level of the noise | +| | | 11: Reserved | +| 4~5 | Return number | 00: return 0 | +| | | 01: return 1 | +| | | 10: return 2 | +| | | 11: return 3 | +| 6~7 | Reserved | | + +You can download more detail description about the livox from external link [1]. + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +[1] + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/livox/livox_tag_filter/include/livox_tag_filter/livox_tag_filter_node.hpp b/sensing/livox/livox_tag_filter/include/livox_tag_filter/livox_tag_filter_node.hpp new file mode 100644 index 0000000000000..8968f61262934 --- /dev/null +++ b/sensing/livox/livox_tag_filter/include/livox_tag_filter/livox_tag_filter_node.hpp @@ -0,0 +1,46 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIVOX_TAG_FILTER__LIVOX_TAG_FILTER_NODE_HPP_ +#define LIVOX_TAG_FILTER__LIVOX_TAG_FILTER_NODE_HPP_ + +#include + +#include + +#include +#include + +namespace livox_tag_filter +{ +class LivoxTagFilterNode : public rclcpp::Node +{ +public: + explicit LivoxTagFilterNode(const rclcpp::NodeOptions & node_options); + +private: + // Parameter + std::vector ignore_tags_; + + // Subscriber + rclcpp::Subscription::SharedPtr sub_pointcloud_; + + void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + + // Publisher + rclcpp::Publisher::SharedPtr pub_pointcloud_; +}; +} // namespace livox_tag_filter + +#endif // LIVOX_TAG_FILTER__LIVOX_TAG_FILTER_NODE_HPP_ diff --git a/sensing/livox/livox_tag_filter/launch/livox_tag_filter.launch.xml b/sensing/livox/livox_tag_filter/launch/livox_tag_filter.launch.xml new file mode 100644 index 0000000000000..332ecbcb8e050 --- /dev/null +++ b/sensing/livox/livox_tag_filter/launch/livox_tag_filter.launch.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + diff --git a/sensing/livox/livox_tag_filter/package.xml b/sensing/livox/livox_tag_filter/package.xml new file mode 100644 index 0000000000000..7a1ed7e1b855e --- /dev/null +++ b/sensing/livox/livox_tag_filter/package.xml @@ -0,0 +1,23 @@ + + + livox_tag_filter + 0.1.0 + The livox_tag_filter package + Kenji Miyake + Apache License 2.0 + + ament_cmake_auto + + libpcl-all-dev + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/sensing/livox/livox_tag_filter/src/livox_tag_filter_node/livox_tag_filter_node.cpp b/sensing/livox/livox_tag_filter/src/livox_tag_filter_node/livox_tag_filter_node.cpp new file mode 100644 index 0000000000000..2fd55ce8df6a0 --- /dev/null +++ b/sensing/livox/livox_tag_filter/src/livox_tag_filter_node/livox_tag_filter_node.cpp @@ -0,0 +1,90 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "livox_tag_filter/livox_tag_filter_node.hpp" + +#include + +#include +#include +#include + +struct LivoxPoint +{ + float x; + float y; + float z; + float intensity; + std::uint8_t tag; + std::uint8_t line; +} EIGEN_ALIGN16; + +POINT_CLOUD_REGISTER_POINT_STRUCT( + LivoxPoint, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( + std::uint8_t, tag, tag)(std::uint8_t, line, line)) + +namespace livox_tag_filter +{ +LivoxTagFilterNode::LivoxTagFilterNode(const rclcpp::NodeOptions & node_options) +: Node("livox_tag_filter", node_options) +{ + // Parameter + ignore_tags_ = this->declare_parameter("ignore_tags", std::vector{}); + + // Subscriber + using std::placeholders::_1; + sub_pointcloud_ = this->create_subscription( + "input", rclcpp::SensorDataQoS(), std::bind(&LivoxTagFilterNode::onPointCloud, this, _1)); + + // Publisher + pub_pointcloud_ = + this->create_publisher("output", rclcpp::SensorDataQoS()); +} + +void LivoxTagFilterNode::onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + pcl::PointCloud points; + pcl::fromROSMsg(*msg, points); + + const auto isIgnored = [&](const LivoxPoint & p) { + for (const auto & ignore_tag : ignore_tags_) { + if (p.tag == ignore_tag) { + return true; + } + } + + return false; + }; + + pcl::PointCloud tag_filtered_points; + for (const auto & p : points) { + if (isIgnored(p)) { + continue; + } + + tag_filtered_points.push_back(p); + } + + // Publish ROS message + auto tag_filtered_msg_ptr = std::make_unique(); + pcl::toROSMsg(tag_filtered_points, *tag_filtered_msg_ptr); + tag_filtered_msg_ptr->header = msg->header; + + pub_pointcloud_->publish(std::move(tag_filtered_msg_ptr)); +} + +} // namespace livox_tag_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(livox_tag_filter::LivoxTagFilterNode) diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt new file mode 100644 index 0000000000000..d5ebd3193c6b2 --- /dev/null +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -0,0 +1,169 @@ +cmake_minimum_required(VERSION 3.5) +project(pointcloud_preprocessor) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +# Ignore PCL errors in Clang +if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-gnu-anonymous-struct -Wno-nested-anon-types) +endif() + +find_package(ament_cmake_auto REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(OpenMP) +ament_auto_find_build_dependencies() + + +########### +## Build ## +########### + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${GRID_MAP_INCLUDE_DIR} +) + +ament_auto_add_library(pointcloud_preprocessor_filter SHARED + src/filter.cpp + src/concatenate_data/concatenate_data_nodelet.cpp + src/crop_box_filter/crop_box_filter_nodelet.cpp + src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp + src/downsample_filter/random_downsample_filter_nodelet.cpp + src/downsample_filter/approximate_downsample_filter_nodelet.cpp + src/outlier_filter/ring_outlier_filter_nodelet.cpp + src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp + src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp + src/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.cpp + src/outlier_filter/dual_return_outlier_filter_nodelet.cpp + src/passthrough_filter/passthrough_filter_nodelet.cpp + src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp + src/passthrough_filter/passthrough_uint16.cpp + src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp + src/vector_map_filter/lanelet2_map_filter_nodelet.cpp + src/distortion_corrector/distortion_corrector.cpp +) + +target_link_libraries(pointcloud_preprocessor_filter + ${Boost_LIBRARIES} + ${OpenCV_LIBRARIES} + ${PCL_LIBRARIES} +) + +if(OPENMP_FOUND) + set_target_properties(pointcloud_preprocessor_filter PROPERTIES + COMPILE_FLAGS ${OpenMP_CXX_FLAGS} + LINK_FLAGS ${OpenMP_CXX_FLAGS} + ) +endif() + +# ========== Concatenate data ========== +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent" + EXECUTABLE concatenate_data_node) + + +# ========== CropBox ========== +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::CropBoxFilterComponent" + EXECUTABLE crop_box_filter_node) + + +# ========== Down Sampler Filter ========== +# -- Voxel Grid Downsample Filter -- +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::VoxelGridDownsampleFilterComponent" + EXECUTABLE voxel_grid_downsample_filter_node) + +# -- Random Downsample Filter -- +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::RandomDownsampleFilterComponent" + EXECUTABLE random_downsample_filter_node) + +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::ApproximateDownsampleFilterComponent" + EXECUTABLE approximate_downsample_filter_node) + +# ========== Outlier Filter ========== +# -- Ring Outlier Filter -- +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::RingOutlierFilterComponent" + EXECUTABLE ring_outlier_filter_node) + +# -- Voxel Grid Outlier Filter -- +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::VoxelGridOutlierFilterComponent" + EXECUTABLE voxel_grid_outlier_filter_node) + +# -- Radius Search 2D Outlier Filter -- +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::RadiusSearch2DOutlierFilterComponent" + EXECUTABLE radius_search_2d_outlier_filter_node) + +# -- Occupancy Grid Map Outlier Filter -- +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::OccupancyGridMapOutlierFilterComponent" + EXECUTABLE occupancy_grid_map_outlier_filter_node) + +# -- DualReturn Outlier Filter-- +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::DualReturnOutlierFilterComponent" + EXECUTABLE dual_return_outlier_filter_node) + +# ========== Passthrough Filter ========== +# -- Passthrough Filter -- +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::PassThroughFilterComponent" + EXECUTABLE passthrough_filter_node) + +# -- Passthrough Filter Uint16 -- +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::PassThroughFilterUInt16Component" + EXECUTABLE passthrough_filter_uint16_node) + + +# ========== Pointcloud Accumulator Filter ========== +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::PointcloudAccumulatorComponent" + EXECUTABLE pointcloud_accumulator_node) + + +# ========== Vector Map Filter ========== +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::Lanelet2MapFilterComponent" + EXECUTABLE vector_map_filter_node) + + +# ========== Distortion Corrector ========== +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::DistortionCorrectorComponent" + EXECUTABLE distortion_corrector_node) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +############# +## Install ## +############# + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/sensing/pointcloud_preprocessor/README.md b/sensing/pointcloud_preprocessor/README.md new file mode 100644 index 0000000000000..03199fdf6d9d9 --- /dev/null +++ b/sensing/pointcloud_preprocessor/README.md @@ -0,0 +1,64 @@ +# pointcloud_preprocessor + +## Purpose + +The `pointcloud_preprocessor` is a package that includes the following filters: + +- removing outlier points +- cropping +- concatenating pointclouds +- correcting distortion +- downsampling + +## Inner-workings / Algorithms + +Detail description of each filter's algorithm is in the following links. + +| Filter Name | Description | Detail | +| ---------------------- | ---------------------------------------------------------------------------------- | -------------------------------------- | +| concatenate_data | subscribe multiple pointclouds and concatenate them into a pointcloud | [link](docs/concatenate-data.md) | +| crop_box_filter | remove points within a given box | [link](docs/crop-box-filter.md) | +| distortion_corrector | compensate pointcloud distortion caused by ego vehicle's movement during 1 scan | [link](docs/distortion-corrector.md) | +| downsample_filter | downsampling input pointcloud | [link](docs/downsample-filter.md) | +| outlier_filter | remove points caused by hardware problems, rain drops and small insects as a noise | [link](docs/outlier-filter.md) | +| passthrough_filter | remove points on the outside of a range in given field (e.g. x, y, z, intensity) | [link](docs/passthrough-filter.md) | +| pointcloud_accumulator | accumulate pointclouds for a given amount of time | [link](docs/pointcloud-accumulator.md) | +| vector_map_filter | remove points on the outside of lane by using vector map | [link](docs/vector-map-filter.md) | + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ----------------- | ------------------------------- | ----------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/indices` | `pcl_msgs::msg::Indices` | reference indices | + +### Output + +| Name | Type | Description | +| ----------------- | ------------------------------- | --------------- | +| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points | + +## Parameters + +### Node Parameters + +| Name | Type | Default Value | Description | +| ------------------ | ------ | ------------- | ------------------------------------- | +| `input_frame` | string | " " | input frame id | +| `output_frame` | string | " " | output frame id | +| `max_queue_size` | int | 5 | max queue size of input/output topics | +| `use_indices` | bool | false | flag to use pointcloud indices | +| `latched_indices` | bool | false | flag to latch pointcloud indices | +| `approximate_sync` | bool | false | flag to use approximate sync option | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md new file mode 100644 index 0000000000000..f59ff75852b1b --- /dev/null +++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md @@ -0,0 +1,46 @@ +# concatenate_data + +## Purpose + +The `concatenate_data` is a node that concatenates multiple pointclouds acquired by multiple LiDARs into a pointcloud. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------- | ------------------------------------------------- | ---------------- | +| `~/input/points` | `sensor_msgs::msg::Pointcloud2` | reference points | +| `~/input/twist` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | vehicle velocity | + +### Output + +| Name | Type | Description | +| ----------------- | ------------------------------- | --------------- | +| `~/output/points` | `sensor_msgs::msg::Pointcloud2` | filtered points | + +## Parameters + +| Name | Type | Default Value | Description | +| ---------------- | ------ | ------------- | ------------------------------------- | +| `input_frame` | string | " " | input frame id | +| `output_frame` | string | " " | output frame id | +| `max_queue_size` | int | 5 | max queue size of input/output topics | + +### Core Parameters + +| Name | Type | Default Value | Description | +| ------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/pointcloud_preprocessor/docs/crop-box-filter.md b/sensing/pointcloud_preprocessor/docs/crop-box-filter.md new file mode 100644 index 0000000000000..edfa726a00b5a --- /dev/null +++ b/sensing/pointcloud_preprocessor/docs/crop-box-filter.md @@ -0,0 +1,42 @@ +# box_crop_filter + +## Purpose + +The `box_crop_filter` is a node that removes points with in a given box region. This filter is used to remove the points that hit the vehicle itself. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +| Name | Type | Description | +| ---------------- | ------------------------------- | ---------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | + +### Output + +| Name | Type | Description | +| ----------------- | ------------------------------- | --------------- | +| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| ------- | ------ | ------------- | ----------------------------------------- | +| `min_x` | double | -1.0 | x-coordinate minimum value for crop range | +| `max_x` | double | 1.0 | x-coordinate maximum value for crop range | +| `min_y` | double | -1.0 | y-coordinate minimum value for crop range | +| `max_y` | double | 1.0 | y-coordinate maximum value for crop range | +| `min_z` | double | -1.0 | z-coordinate minimum value for crop range | +| `max_z` | double | 1.0 | z-coordinate maximum value for crop range | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md new file mode 100644 index 0000000000000..7fbce4d447260 --- /dev/null +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -0,0 +1,40 @@ +# distortion_corrector + +## Purpose + +The `distortion_corrector` is a node that compensates pointcloud distortion caused by ego vehicle's movement during 1 scan. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------- | ------------------------------------------------- | ---------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/velocity_report` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | vehicle velocity | + +### Output + +| Name | Type | Description | +| ----------------- | ------------------------------- | --------------- | +| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| ---------------------- | ------ | ------------- | --------------------- | +| `timestamp_field_name` | string | "time_stamp" | time stamp field name | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/pointcloud_preprocessor/docs/downsample-filter.md b/sensing/pointcloud_preprocessor/docs/downsample-filter.md new file mode 100644 index 0000000000000..8902b9fb3af45 --- /dev/null +++ b/sensing/pointcloud_preprocessor/docs/downsample-filter.md @@ -0,0 +1,52 @@ +# downsample_filter + +## Purpose + +The `downsample_filter` is a node that reduces the number of points. + +## Inner-workings / Algorithms + +### Approximate Downsample Filter + +WIP + +### Random Downsample Filter + +WIP + +### Voxel Grid Downsample Filter + +WIP + +## Inputs / Outputs + +| Name | Type | Description | +| ---------------- | ------------------------------- | ---------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | + +### Output + +| Name | Type | Description | +| ----------------- | ------------------------------- | --------------- | +| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points | + +## Parameters + +### Node Parameters + +| Name | Type | Default Value | Description | +| -------------- | ------ | ------------- | ------------------------------- | +| `voxel_size_x` | double | 0.3 | voxel size x [m] | +| `voxel_size_y` | double | 0.3 | voxel size y [m] | +| `voxel_size_z` | double | 0.1 | voxel size z [m] | +| `sample_num` | int | 1500 | number of indices to be sampled | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/pointcloud_preprocessor/docs/outlier-filter.md b/sensing/pointcloud_preprocessor/docs/outlier-filter.md new file mode 100644 index 0000000000000..2487a988bc717 --- /dev/null +++ b/sensing/pointcloud_preprocessor/docs/outlier-filter.md @@ -0,0 +1,81 @@ +# outlier_filter + +## Purpose + +This node is an outlier filter based on a occupancy grid map. +Depending on the implementation of occupancy grid map, it can be called an outlier filter in time series, since the occupancy grid map expresses the occupancy probabilities in time series. + +## Inner-workings / Algorithms + +### Dual Return Outlier Filter + +WIP + +### Occupancy GridMap Outlier Filter + +1. Use the occupancy grid map to separate point clouds into those with low occupancy probability and those with high occupancy probability. + +2. The point clouds that belong to the low occupancy probability are not necessarily outliers. In particular, the top of the moving object tends to belong to the low occupancy probability. Therefore, if `use_radius_search_2d_filter` is true, then apply an radius search 2d outlier filter to the point cloud that is determined to have a low occupancy probability. + 1. For each low occupancy probability point, determine the outlier from the radius (`radius_search_2d_filter/search_radius`) and the number of point clouds. In this case, the point cloud to be referenced is not only low occupancy probability points, but all point cloud including high occupancy probability points. + 2. The number of point clouds can be multiplied by `radius_search_2d_filter/min_points_and_distance_ratio` and distance from base link. However, the minimum and maximum number of point clouds is limited. + +The following video is a sample. Yellow points are high occupancy probability, green points are low occupancy probability which is not an outlier, and red points are outliers. At around 0:15 and 1:16 in the first video, a bird crosses the road, but it is considered as an outlier. + +- [movie1](https://www.youtube.com/watch?v=hEVv0LaTpP8) +- [movie2](https://www.youtube.com/watch?v=VaHs1CdLcD0) + +### Radius Search 2d Outlier Filter [1] + +WIP + +### Ring Outlier Filter + +WIP + +### Voxel Grid Outlier Filter + +WIP + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------------------- | ------------------------- | ------------------------------------------------------------------------------------------ | +| `~/input/pointcloud` | `sensor_msgs/PointCloud2` | Obstacle point cloud with ground removed. | +| `~/input/occupancy_grid_map` | `nav_msgs/OccupancyGrid` | A map in which the probability of the presence of an obstacle is occupancy probability map | + +### Output + +| Name | Type | Description | +| ------------------------------------------- | ------------------------- | ---------------------------------------------------------------------------------------------------------------------------- | +| `~/output/pointcloud` | `sensor_msgs/PointCloud2` | Point cloud with outliers removed. trajectory | +| `~/output/debug/outlier/pointcloud` | `sensor_msgs/PointCloud2` | Point clouds removed as outliers. | +| `~/output/debug/low_confidence/pointcloud` | `sensor_msgs/PointCloud2` | Point clouds that had a low probability of occupancy in the occupancy grid map. However, it is not considered as an outlier. | +| `~/output/debug/high_confidence/pointcloud` | `sensor_msgs/PointCloud2` | Point clouds that had a high probability of occupancy in the occupancy grid map. trajectory | + +## Parameters + +| Name | Type | Description | +| ------------------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `map_frame` | string | map frame id | +| `base_link_frame` | string | base link frame id | +| `cost_threshold` | int | Cost threshold of occupancy grid map (0~100). 100 means 100% probability that there is an obstacle, close to 50 means that it is indistinguishable whether it is an obstacle or free space, 0 means that there is no obstacle. | +| `enable_debugger` | bool | Whether to output the point cloud for debugging. | +| `use_radius_search_2d_filter` | bool | Whether or not to apply density-based outlier filters to objects that are judged to have low probability of occupancy on the occupancy grid map. | +| `radius_search_2d_filter/search_radius` | float | Radius when calculating the density | +| `radius_search_2d_filter/min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. | +| `radius_search_2d_filter/min_points` | int | Minimum number of point clouds per radius | +| `radius_search_2d_filter/max_points` | int | Maximum number of point clouds per radius | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +[1] + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/pointcloud_preprocessor/docs/passthrough-filter.md b/sensing/pointcloud_preprocessor/docs/passthrough-filter.md new file mode 100644 index 0000000000000..4d28f08da28cc --- /dev/null +++ b/sensing/pointcloud_preprocessor/docs/passthrough-filter.md @@ -0,0 +1,44 @@ +# passthrough_filter + +## Purpose + +The `passthrough_filter` is a node that removes points on the outside of a range in a given field (e.g. x, y, z, intensity, ring, etc). + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ----------------- | ------------------------------- | ----------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/indices` | `pcl_msgs::msg::Indices` | reference indices | + +### Output + +| Name | Type | Description | +| ----------------- | ------------------------------- | --------------- | +| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| ----------------------- | ------ | ------------- | ------------------------------------------------------ | +| `filter_limit_min` | int | 0 | minimum allowed field value | +| `filter_limit_max` | int | 127 | maximum allowed field value | +| `filter_field_name` | string | "ring" | filtering field name | +| `keep_organized` | bool | false | flag to keep indices structure | +| `filter_limit_negative` | bool | false | flag to return whether the data is inside limit or not | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/pointcloud_preprocessor/docs/pointcloud-accumulator.md b/sensing/pointcloud_preprocessor/docs/pointcloud-accumulator.md new file mode 100644 index 0000000000000..d9fa56ca9c995 --- /dev/null +++ b/sensing/pointcloud_preprocessor/docs/pointcloud-accumulator.md @@ -0,0 +1,40 @@ +# pointcloud_accumulator + +## Purpose + +The `pointcloud_accumulator` is a node that accumulates pointclouds for a given amount of time. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------- | ------------------------------- | ---------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | + +### Output + +| Name | Type | Description | +| ----------------- | ------------------------------- | --------------- | +| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| ------------------------ | ------ | ------------- | ----------------------- | +| `accumulation_time_sec` | double | 2.0 | accumulation period [s] | +| `pointcloud_buffer_size` | int | 50 | buffer size | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-filter.md new file mode 100644 index 0000000000000..fedd3c081cdeb --- /dev/null +++ b/sensing/pointcloud_preprocessor/docs/vector-map-filter.md @@ -0,0 +1,41 @@ +# vector_map_filter + +## Purpose + +The `vector_map_filter` is a node that removes points on the outside of lane by using vector map. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------- | -------------------------------------------- | ---------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | + +### Output + +| Name | Type | Description | +| ----------------- | ------------------------------- | --------------- | +| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| -------------- | ------ | ------------- | ----------- | +| `voxel_size_x` | double | 0.04 | voxel size | +| `voxel_size_y` | double | 0.04 | voxel size | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp new file mode 100644 index 0000000000000..590d8f091f862 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp @@ -0,0 +1,178 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +// ROS includes +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pointcloud_preprocessor +{ +/** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data + * synchronizer: it listens for a set of input PointCloud messages on the same topic, + * checks their timestamps, and concatenates their fields together into a single + * PointCloud output message. + * \author Radu Bogdan Rusu + */ +class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node +{ +public: + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + + /** \brief constructor. */ + explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options); + + /** \brief constructor. + * \param queue_size the maximum queue size + */ + PointCloudConcatenateDataSynchronizerComponent( + const rclcpp::NodeOptions & node_options, int queue_size); + + /** \brief Empty destructor. */ + virtual ~PointCloudConcatenateDataSynchronizerComponent() {} + +private: + /** \brief The output PointCloud publisher. */ + rclcpp::Publisher::SharedPtr pub_output_; + + /** \brief The maximum number of messages that we can store in the queue. */ + int maximum_queue_size_ = 3; + + double timeout_sec_ = 0.1; + + std::set not_subscribed_topic_names_; + + /** \brief A vector of subscriber. */ + std::vector::SharedPtr> filters_; + + rclcpp::Subscription::SharedPtr sub_twist_; + + rclcpp::TimerBase::SharedPtr timer_; + diagnostic_updater::Updater updater_{this}; + + /** \brief Output TF frame the concatenated points should be transformed to. */ + std::string output_frame_; + + /** \brief Input point cloud topics. */ + // XmlRpc::XmlRpcValue input_topics_; + std::vector input_topics_; + + /** \brief TF listener object. */ + std::shared_ptr tf2_buffer_; + std::shared_ptr tf2_listener_; + + std::deque twist_ptr_queue_; + + std::map cloud_stdmap_; + std::map cloud_stdmap_tmp_; + std::mutex mutex_; + + std::vector input_offset_; + std::map offset_map_; + + void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); + void combineClouds( + const PointCloud2::ConstSharedPtr & in1, const PointCloud2::ConstSharedPtr & in2, + PointCloud2::SharedPtr & out); + void publish(); + + void removeRADTFields( + const sensor_msgs::msg::PointCloud2 & input_cloud, + sensor_msgs::msg::PointCloud2 & output_cloud); + void setPeriod(const int64_t new_period); + void cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, + const std::string & topic_name); + void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input); + void timer_callback(); + + void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); +}; + +struct PointXYZI +{ + PCL_ADD_POINT4D; + float intensity; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +} // namespace pointcloud_preprocessor + +POINT_CLOUD_REGISTER_POINT_STRUCT( + pointcloud_preprocessor::PointXYZI, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)) + +#endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp new file mode 100644 index 0000000000000..85265bd83e0c1 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp @@ -0,0 +1,91 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +/* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: cropbox.cpp + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" + +#include + +#include + +#include + +namespace pointcloud_preprocessor +{ +class CropBoxFilterComponent : public pointcloud_preprocessor::Filter +{ +protected: + virtual void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); + + void publishCropBoxPolygon(); + +private: + /** \brief The PCL filter implementation used. */ + pcl::CropBox impl_; + rclcpp::Publisher::SharedPtr crop_box_polygon_pub_; + + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit CropBoxFilterComponent(const rclcpp::NodeOptions & options); +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp new file mode 100644 index 0000000000000..d121fdbf269f0 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -0,0 +1,70 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ +#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace pointcloud_preprocessor +{ +using autoware_auto_vehicle_msgs::msg::VelocityReport; +using rcl_interfaces::msg::SetParametersResult; +using sensor_msgs::msg::PointCloud2; + +class DistortionCorrectorComponent : public rclcpp::Node +{ +public: + explicit DistortionCorrectorComponent(const rclcpp::NodeOptions & options); + +private: + void onPointCloud(PointCloud2::UniquePtr points_msg); + void onVelocityReport(const VelocityReport::ConstSharedPtr velocity_report_msg); + bool getTransform( + const std::string & target_frame, const std::string & source_frame, + tf2::Transform * tf2_transform_ptr); + + bool undistortPointCloud( + const std::deque & velocity_report_queue, + const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points); + + rclcpp::Subscription::SharedPtr input_points_sub_; + rclcpp::Subscription::SharedPtr velocity_report_sub_; + rclcpp::Publisher::SharedPtr undistorted_points_pub_; + + tf2_ros::Buffer tf2_buffer_{get_clock()}; + tf2_ros::TransformListener tf2_listener_{tf2_buffer_}; + + std::deque velocity_report_queue_; + + std::string base_link_frame_ = "base_link"; + std::string time_stamp_field_name_; +}; + +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp new file mode 100644 index 0000000000000..a665f82747d2c --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp @@ -0,0 +1,85 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ +#ifndef POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" + +#include + +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +class ApproximateDownsampleFilterComponent : public pointcloud_preprocessor::Filter +{ +protected: + void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +private: + double voxel_size_x_; + double voxel_size_y_; + double voxel_size_z_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit ApproximateDownsampleFilterComponent(const rclcpp::NodeOptions & options); +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp new file mode 100644 index 0000000000000..bdb7a0bc705dc --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp @@ -0,0 +1,83 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" + +#include + +#include + +namespace pointcloud_preprocessor +{ +class RandomDownsampleFilterComponent : public pointcloud_preprocessor::Filter +{ +protected: + void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + +private: + size_t sample_num_; + + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit RandomDownsampleFilterComponent(const rclcpp::NodeOptions & options); +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp new file mode 100644 index 0000000000000..98f4333de3a44 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp @@ -0,0 +1,86 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" + +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +class VoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filter +{ +protected: + void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + +private: + double voxel_size_x_; + double voxel_size_y_; + double voxel_size_z_; + + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit VoxelGridDownsampleFilterComponent(const rclcpp::NodeOptions & options); +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp new file mode 100644 index 0000000000000..194f58a6fe7ea --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -0,0 +1,273 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: filter.h 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__FILTER_HPP_ +#define POINTCLOUD_PREPROCESSOR__FILTER_HPP_ + +#include +#include +#include + +// PCL includes +#include + +#include +#include +// PCL includes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Include TF +#include +#include +#include + +namespace pointcloud_preprocessor +{ +namespace sync_policies = message_filters::sync_policies; + +/** \brief For parameter service callback */ +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +/** \brief @b Filter represents the base filter class. Some generic 3D operations that are + * applicable to all filters are defined here as static methods. \author Radu Bogdan Rusu + */ +class Filter : public rclcpp::Node +{ +public: + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = PointCloud::Ptr; + using PointCloudConstPtr = PointCloud::ConstPtr; + + using PointIndices = pcl_msgs::msg::PointIndices; + using PointIndicesPtr = PointIndices::SharedPtr; + using PointIndicesConstPtr = PointIndices::ConstSharedPtr; + + using ModelCoefficients = pcl_msgs::msg::ModelCoefficients; + using ModelCoefficientsPtr = ModelCoefficients::SharedPtr; + using ModelCoefficientsConstPtr = ModelCoefficients::ConstSharedPtr; + + using IndicesPtr = pcl::IndicesPtr; + using IndicesConstPtr = pcl::IndicesConstPtr; + + using ExactTimeSyncPolicy = + message_filters::Synchronizer>; + using ApproximateTimeSyncPolicy = + message_filters::Synchronizer>; + + explicit Filter( + const std::string & filter_name = "pointcloud_preprocessor_filter", + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +protected: + /** \brief The input PointCloud2 subscriber. */ + rclcpp::Subscription::SharedPtr sub_input_; + + /** \brief The output PointCloud2 publisher. */ + rclcpp::Publisher::SharedPtr pub_output_; + + /** \brief The message filter subscriber for PointCloud2. */ + message_filters::Subscriber sub_input_filter_; + + /** \brief The message filter subscriber for PointIndices. */ + message_filters::Subscriber sub_indices_filter_; + + /** \brief The desired user filter field name. */ + std::string filter_field_name_; + + /** \brief The minimum allowed filter value a point will be considered from. */ + double filter_limit_min_; + + /** \brief The maximum allowed filter value a point will be considered from. */ + double filter_limit_max_; + + /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a + * filter_limit_max_). Default: false. */ + bool filter_limit_negative_; + + /** \brief The input TF frame the data should be transformed into, + * if input.header.frame_id is different. */ + std::string tf_input_frame_; + + /** \brief The original data input TF frame. */ + std::string tf_input_orig_frame_; + + /** \brief The output TF frame the data should be transformed into, + * if input.header.frame_id is different. */ + std::string tf_output_frame_; + + /** \brief Internal mutex. */ + boost::mutex mutex_; + + /** \brief Virtual abstract filter method. To be implemented by every child. + * \param input the input point cloud dataset. + * \param indices a pointer to the vector of point indices to use. + * \param output the resultant filtered PointCloud2 + */ + virtual void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) = 0; + + /** \brief Lazy transport subscribe routine. */ + virtual void subscribe(); + + /** \brief Lazy transport unsubscribe routine. */ + virtual void unsubscribe(); + + /** \brief Call the child filter () method, optionally transform the result, and publish it. + * \param input the input point cloud dataset. + * \param indices a pointer to the vector of point indices to use. + */ + void computePublish(const PointCloud2ConstPtr & input, const IndicesPtr & indices); + + ////////////////////// + // from PCLNodelet // + ////////////////////// + /** \brief Set to true if point indices are used. + * + * When receiving a point cloud, if use_indices_ is false, the entire + * point cloud is processed for the given operation. If use_indices_ is + * true, then the ~indices topic is read to get the vector of point + * indices specifying the subset of the point cloud that will be used for + * the operation. In the case where use_indices_ is true, the ~input and + * ~indices topics must be synchronised in time, either exact or within a + * specified jitter. See also @ref latched_indices_ and approximate_sync. + **/ + bool use_indices_ = false; + /** \brief Set to true if the indices topic is latched. + * + * If use_indices_ is true, the ~input and ~indices topics generally must + * be synchronised in time. By setting this flag to true, the most recent + * value from ~indices can be used instead of requiring a synchronised + * message. + **/ + bool latched_indices_ = false; + + /** \brief The maximum queue size (default: 3). */ + size_t max_queue_size_ = 3; + + /** \brief True if we use an approximate time synchronizer + * versus an exact one (false by default). */ + bool approximate_sync_ = false; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + inline bool isValid( + const PointCloud2ConstPtr & cloud, const std::string & /*topic_name*/ = "input") + { + if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) { + RCLCPP_WARN( + this->get_logger(), + "Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, " + "and frame %s received!", + cloud->data.size(), cloud->width, cloud->height, cloud->point_step, + rclcpp::Time(cloud->header.stamp).seconds(), cloud->header.frame_id.c_str()); + return false; + } + return true; + } + + inline bool isValid( + const PointIndicesConstPtr & /*indices*/, const std::string & /*topic_name*/ = "indices") + { + return true; + } + + inline bool isValid( + const ModelCoefficientsConstPtr & /*model*/, const std::string & /*topic_name*/ = "model") + { + return true; + } + +private: + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_filter_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult filterParamCallback( + const std::vector & p); + + /** \brief Synchronized input, and indices.*/ + std::shared_ptr sync_input_indices_e_; + std::shared_ptr sync_input_indices_a_; + + /** \brief PointCloud2 + Indices data callback. */ + void input_indices_callback(const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices); + + void setupTF(); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__FILTER_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp new file mode 100644 index 0000000000000..b5cc65daeb30b --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp @@ -0,0 +1,106 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; + +enum ReturnType : uint8_t { + INVALID = 0, + SINGLE_STRONGEST, + SINGLE_LAST, + DUAL_STRONGEST_FIRST, + DUAL_STRONGEST_LAST, + DUAL_WEAK_FIRST, + DUAL_WEAK_LAST, + DUAL_ONLY, +}; + +class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter +{ +protected: + virtual void filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output); + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + image_transport::Publisher image_pub_; + rclcpp::Publisher::SharedPtr visibility_pub_; + rclcpp::Publisher::SharedPtr noise_cloud_pub_; + +private: + void onVisibilityChecker(DiagnosticStatusWrapper & stat); + Updater updater_{this}; + double visibility_ = 1.f; + double weak_first_distance_ratio_; + double general_distance_ratio_; + int weak_first_local_noise_threshold_; + double visibility_threshold_; + int vertical_bins_; + float max_azimuth_diff_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit DualReturnOutlierFilterComponent(const rclcpp::NodeOptions & options); +}; + +} // namespace pointcloud_preprocessor + +namespace return_type_cloud +{ +struct PointXYZIRADT +{ + PCL_ADD_POINT4D; + float intensity; + std::uint16_t ring; + float azimuth; + float distance; + std::uint8_t return_type; + double time_stamp; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +} // namespace return_type_cloud + +POINT_CLOUD_REGISTER_POINT_STRUCT( + return_type_cloud::PointXYZIRADT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( + float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( + double, time_stamp, time_stamp)) + +#endif // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp new file mode 100644 index 0000000000000..e794bebdc1520 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -0,0 +1,126 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace pointcloud_preprocessor +{ +using geometry_msgs::msg::Pose; +using nav_msgs::msg::OccupancyGrid; +using sensor_msgs::msg::PointCloud2; +using std_msgs::msg::Header; +using PclPointCloud = pcl::PointCloud; + +class RadiusSearch2dfilter +{ +public: + explicit RadiusSearch2dfilter(rclcpp::Node & node); + void filter( + const PclPointCloud & input, const Pose & pose, PclPointCloud & output, + PclPointCloud & outlier); + void filter( + const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose, + PclPointCloud & output, PclPointCloud & outlier); + +private: + float search_radius_; + float min_points_and_distance_ratio_; + int min_points_; + int max_points_; + pcl::search::Search::Ptr kd_tree_; +}; + +class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node +{ +public: + explicit OccupancyGridMapOutlierFilterComponent(const rclcpp::NodeOptions & options); + +private: + void onOccupancyGridMapAndPointCloud2( + const OccupancyGrid::ConstSharedPtr & input_occupancy_grid_map, + const PointCloud2::ConstSharedPtr & input_pointcloud); + void filterByOccupancyGridMap( + const OccupancyGrid & occupancy_grid_map, const PointCloud2 & pointcloud, + PclPointCloud & high_confidence, PclPointCloud & low_confidence); + +private: + class Debugger + { + // *INDENT-OFF* + public: + // *INDENT-ON* + explicit Debugger(OccupancyGridMapOutlierFilterComponent & node); + void publishOutlier(const PclPointCloud & input, const Header & header); + void publishHighConfidence(const PclPointCloud & input, const Header & header); + void publishLowConfidence(const PclPointCloud & input, const Header & header); + // *INDENT-OFF* + private: + // *INDENT-ON* + void transformToBaseLink( + const PclPointCloud & input, const Header & header, PointCloud2 & output); + rclcpp::Publisher::SharedPtr outlier_pointcloud_pub_; + rclcpp::Publisher::SharedPtr low_confidence_pointcloud_pub_; + rclcpp::Publisher::SharedPtr high_confidence_pointcloud_pub_; + const OccupancyGridMapOutlierFilterComponent & node_; + }; + +private: + // publishers and subscribers + rclcpp::Publisher::SharedPtr pointcloud_pub_; + message_filters::Subscriber occupancy_grid_map_sub_; + message_filters::Subscriber pointcloud_sub_; + using SyncPolicy = message_filters::sync_policies::ExactTime; + using Sync = message_filters::Synchronizer; + std::shared_ptr sync_ptr_; + + // tf + std::shared_ptr tf2_; + std::shared_ptr tf2_listener_; + + // 2d outlier filter + std::shared_ptr radius_search_2d_filter_ptr_; + + // Debugger + std::shared_ptr debugger_ptr_; + + // ROS Parameters + std::string map_frame_; + std::string base_link_frame_; + int cost_threshold_; +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp new file mode 100644 index 0000000000000..53f2bf45e1046 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp @@ -0,0 +1,57 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" + +#include + +#include +#include +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +class RadiusSearch2DOutlierFilterComponent : public pointcloud_preprocessor::Filter +{ +protected: + virtual void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); + +private: + double search_radius_; + size_t min_neighbors_; + + // pcl::RadiusOutlierRemoval radius_outlier_removal_; + pcl::search::Search::Ptr kd_tree_; + // pcl::ExtractIndices extract_indices_; + + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit RadiusSearch2DOutlierFilterComponent(const rclcpp::NodeOptions & options); +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp new file mode 100644 index 0000000000000..6f88d6cf2bed8 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -0,0 +1,81 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" + +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter +{ +protected: + virtual void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); + +private: + double distance_ratio_; + double object_length_threshold_; + int num_points_threshold_; + + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options); +}; + +} // namespace pointcloud_preprocessor + +namespace custom_pcl +{ +struct PointXYZIRADT +{ + PCL_ADD_POINT4D; + float intensity; + std::uint16_t ring; + float azimuth; + float distance; + double time_stamp; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +struct PointXYZI +{ + PCL_ADD_POINT4D; + float intensity; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +} // namespace custom_pcl + +POINT_CLOUD_REGISTER_POINT_STRUCT( + custom_pcl::PointXYZIRADT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( + float, azimuth, azimuth)(float, distance, distance)(double, time_stamp, time_stamp)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + custom_pcl::PointXYZI, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)) + +#endif // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp new file mode 100644 index 0000000000000..05f7514c1e408 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp @@ -0,0 +1,53 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" + +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +class VoxelGridOutlierFilterComponent : public pointcloud_preprocessor::Filter +{ +protected: + virtual void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); + + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +private: + double voxel_size_x_; + double voxel_size_y_; + double voxel_size_z_; + int voxel_points_threshold_; + + pcl::VoxelGrid voxel_filter; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit VoxelGridOutlierFilterComponent(const rclcpp::NodeOptions & option); +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp new file mode 100644 index 0000000000000..e34cdba64b929 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp @@ -0,0 +1,80 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: passthrough.cpp 36194 2011-02-23 07:49:21Z rusu $ + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" + +#include + +#include + +namespace pointcloud_preprocessor +{ +class PassThroughFilterComponent : public pointcloud_preprocessor::Filter +{ +protected: + virtual void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); + + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit PassThroughFilterComponent(const rclcpp::NodeOptions & options); +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp new file mode 100644 index 0000000000000..35e8dfdec7d3d --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp @@ -0,0 +1,48 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp" + +#include + +#include + +namespace pointcloud_preprocessor +{ +class PassThroughFilterUInt16Component : public pointcloud_preprocessor::Filter +{ +protected: + virtual void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); + + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +private: + pcl::PassThroughUInt16 impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit PassThroughFilterUInt16Component(const rclcpp::NodeOptions & options); +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp new file mode 100644 index 0000000000000..266aa13b06bbb --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp @@ -0,0 +1,473 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ +#define POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ + +#include +#include + +#include +#include +#include + +namespace pcl +{ +/** \brief @b PassThroughUInt16 passes points in a cloud based on constraints for one particular + * field of the point type. \details Iterates through the entire input once, automatically filtering + * non-finite points and the points outside the interval specified by setFilterLimits(), which + * applies only to the field specified by setFilterFieldName().

Usage example: \code + * pcl::PassThroughUInt16 ptfilter (true); // Initializing with true will allow us to + * extract the removed indices ptfilter.setInputCloud (cloud_in); ptfilter.setFilterFieldName ("x"); + * ptfilter.setFilterLimits (0.0, 1000.0); + * ptfilter.filter (*indices_x); + * // The indices_x array indexes all points of cloud_in that have x between 0.0 and 1000.0 + * indices_rem = ptfilter.getRemovedIndices (); + * // The indices_rem array indexes all points of cloud_in that have x smaller than 0.0 or larger + * than 1000.0 + * // and also indexes all non-finite points of cloud_in + * ptfilter.setIndices (indices_x); + * ptfilter.setFilterFieldName ("z"); + * ptfilter.setFilterLimits (-10.0, 10.0); + * ptfilter.setNegative (true); + * ptfilter.filter (*indices_xz); + * // The indices_xz array indexes all points of cloud_in that have x between 0.0 and 1000.0 and z + * larger than 10.0 or smaller than -10.0 ptfilter.setIndices (indices_xz); + * ptfilter.setFilterFieldName ("intensity"); + * ptfilter.setFilterLimits (FLT_MIN, 0.5); + * ptfilter.setNegative (false); + * ptfilter.filter (*cloud_out); + * // The resulting cloud_out contains all points of cloud_in that are finite and have: + * // x between 0.0 and 1000.0, z larger than 10.0 or smaller than -10.0 and intensity smaller than + * 0.5. \endcode \author Radu Bogdan Rusu \ingroup filters + */ +template +class PassThroughUInt16 : public FilterIndices +{ +protected: + typedef typename FilterIndices::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + typedef typename pcl::traits::fieldList::type FieldList; + +public: + typedef boost::shared_ptr> Ptr; + typedef boost::shared_ptr> ConstPtr; + + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of + * points being removed (default = false). + */ + explicit PassThroughUInt16(bool extract_removed_indices = false) + : FilterIndices::FilterIndices(extract_removed_indices), + filter_field_name_(""), + filter_limit_min_(0), + filter_limit_max_(UINT16_MAX) + { + filter_name_ = "PassThroughUInt16"; + } + + /** \brief Provide the name of the field to be used for filtering data. + * \details In conjunction with setFilterLimits(), points having values outside this interval for + * this field will be discarded. \param[in] field_name The name of the field that will be used for + * filtering. + */ + inline void setFilterFieldName(const std::string & field_name) + { + filter_field_name_ = field_name; + } + + /** \brief Retrieve the name of the field to be used for filtering data. + * \return The name of the field that will be used for filtering. + */ + inline std::string const getFilterFieldName() { return filter_field_name_; } + + /** \brief Set the numerical limits for the field for filtering data. + * \details In conjunction with setFilterFieldName(), points having values outside this interval + * for this field will be discarded. \param[in] limit_min The minimum allowed field value (default + * = FLT_MIN). \param[in] limit_max The maximum allowed field value (default = FLT_MAX). + */ + inline void setFilterLimits(const std::uint16_t & limit_min, const std::uint16_t & limit_max) + { + filter_limit_min_ = limit_min; + filter_limit_max_ = limit_max; + } + + /** \brief Get the numerical limits for the field for filtering data. + * \param[out] limit_min The minimum allowed field value (default = FLT_MIN). + * \param[out] limit_max The maximum allowed field value (default = FLT_MAX). + */ + inline void getFilterLimits(std::uint16_t & limit_min, std::uint16_t & limit_max) + { + limit_min = filter_limit_min_; + limit_max = filter_limit_max_; + } + + /** \brief Set to true if we want to return the data outside the interval specified by + * setFilterLimits (min, max) Default: false. \warning This method will be removed in the future. + * Use setNegative() instead. \param[in] limit_negative return data inside the interval (false) or + * outside (true) + */ + inline void setFilterLimitsNegative(const bool limit_negative) { negative_ = limit_negative; } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside + * (false). \warning This method will be removed in the future. Use getNegative() instead. + * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, + * false otherwise + */ + inline void getFilterLimitsNegative(bool & limit_negative) { limit_negative = negative_; } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside + * (false). \warning This method will be removed in the future. Use getNegative() instead. \return + * true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline bool getFilterLimitsNegative() { return negative_; } + +protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Filtered results are stored in a separate point cloud. + * \param[out] output The resultant point cloud. + */ + void applyFilter(PointCloud & output); + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void applyFilter(std::vector & indices) { applyFilterIndices(indices); } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void applyFilterIndices(std::vector & indices); + +private: + /** \brief The name of the field that will be used for filtering. */ + std::string filter_field_name_; + + /** \brief The minimum allowed field value (default = FLT_MIN). */ + std::uint16_t filter_limit_min_; + + /** \brief The maximum allowed field value (default = FLT_MIN). */ + std::uint16_t filter_limit_max_; +}; + +//////////////////////////////////////////////////////////////////////////////////////////// +/** \brief PassThroughUInt16 uses the base Filter class methods to pass through all data that + * satisfies the user given constraints. \author Radu B. Rusu \ingroup filters + */ +template <> +class PCL_EXPORTS PassThroughUInt16 : public Filter +{ + typedef pcl::PCLPointCloud2 PCLPointCloud2; + typedef PCLPointCloud2::Ptr PCLPointCloud2Ptr; + typedef PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr; + + using Filter::removed_indices_; + using Filter::extract_removed_indices_; + +public: + /** \brief Constructor. */ + explicit PassThroughUInt16(bool extract_removed_indices = false) + : Filter::Filter(extract_removed_indices), + keep_organized_(false), + user_filter_value_(std::numeric_limits::quiet_NaN()), + filter_field_name_(""), + filter_limit_min_(0), + filter_limit_max_(UINT16_MAX), + filter_limit_negative_(false) + { + filter_name_ = "PassThroughUInt16"; + } + + /** \brief Set whether the filtered points should be kept and set to the + * value given through \a setUserFilterValue (default: NaN), or removed + * from the PointCloud, thus potentially breaking its organized + * structure. By default, points are removed. + * + * \param[in] val set to true whether the filtered points should be kept and + * set to a user given value (default: NaN) + */ + inline void setKeepOrganized(bool val) { keep_organized_ = val; } + + /** \brief Obtain the value of the internal \a keep_organized_ parameter. */ + inline bool getKeepOrganized() { return keep_organized_; } + + /** \brief Provide a value that the filtered points should be set to + * instead of removing them. Used in conjunction with \a + * setKeepOrganized (). + * \param[in] val the user given value that the filtered point dimensions should be set to + */ + inline void setUserFilterValue(float val) { user_filter_value_ = val; } + + /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a + * setFilterLimits, points having values outside this interval will be discarded. \param[in] + * field_name the name of the field that contains values used for filtering + */ + inline void setFilterFieldName(const std::string & field_name) + { + filter_field_name_ = field_name; + } + + /** \brief Get the name of the field used for filtering. */ + inline std::string const getFilterFieldName() { return filter_field_name_; } + + /** \brief Set the field filter limits. All points having field values outside this interval will + * be discarded. \param[in] limit_min the minimum allowed field value \param[in] limit_max the + * maximum allowed field value + */ + inline void setFilterLimits(const std::uint16_t & limit_min, const std::uint16_t & limit_max) + { + filter_limit_min_ = limit_min; + filter_limit_max_ = limit_max; + } + + /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, + * FLT_MAX. \param[out] limit_min the minimum allowed field value \param[out] limit_max the + * maximum allowed field value + */ + inline void getFilterLimits(std::uint16_t & limit_min, std::uint16_t & limit_max) + { + limit_min = filter_limit_min_; + limit_max = filter_limit_max_; + } + + /** \brief Set to true if we want to return the data outside the interval specified by + * setFilterLimits (min, max). Default: false. \param[in] limit_negative return data inside the + * interval (false) or outside (true) + */ + inline void setFilterLimitsNegative(const bool limit_negative) + { + filter_limit_negative_ = limit_negative; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside + * (false). \param[out] limit_negative true if data \b outside the interval [min; max] is to be + * returned, false otherwise + */ + inline void getFilterLimitsNegative(bool & limit_negative) + { + limit_negative = filter_limit_negative_; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside + * (false). \return true if data \b outside the interval [min; max] is to be returned, false + * otherwise + */ + inline bool getFilterLimitsNegative() { return filter_limit_negative_; } + +protected: + void applyFilter(PCLPointCloud2 & output); + +private: + /** \brief Keep the structure of the data organized, by setting the + * filtered points to a user given value (NaN by default). + */ + bool keep_organized_; + + /** \brief User given value to be set to any filtered point. Casted to + * the correct field type. + */ + float user_filter_value_; + + /** \brief The desired user filter field name. */ + std::string filter_field_name_; + + /** \brief The minimum allowed filter value a point will be considered from. */ + std::uint16_t filter_limit_min_; + + /** \brief The maximum allowed filter value a point will be considered from. */ + std::uint16_t filter_limit_max_; + + /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a + * filter_limit_max_). Default: false. */ + bool filter_limit_negative_; +}; +} // namespace pcl + +template +void pcl::PassThroughUInt16::applyFilter(PointCloud & output) +{ + std::vector indices; + if (keep_organized_) { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilterIndices(indices); + extract_removed_indices_ = temp; + + output = *input_; + for (int rii = 0; rii < static_cast(removed_indices_->size()); + ++rii) // rii = removed indices iterator + { + output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = + output.points[(*removed_indices_)[rii]].z = user_filter_value_; + } + if (!std::isfinite(user_filter_value_)) { + output.is_dense = false; + } + } else { + output.is_dense = true; + applyFilterIndices(indices); + copyPointCloud(*input_, indices, output); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void pcl::PassThroughUInt16::applyFilterIndices(std::vector & indices) +{ + // The arrays to be used + indices.resize(indices_->size()); + removed_indices_->resize(indices_->size()); + int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator + + // Has a field name been specified? + if (filter_field_name_.empty()) { + // Only filter for non-finite entries then + for (int iii = 0; iii < static_cast(indices_->size()); + ++iii) // iii = input indices iterator + { + // Non-finite entries are always passed to removed indices + if ( + !std::isfinite(input_->points[(*indices_)[iii]].x) || + !std::isfinite(input_->points[(*indices_)[iii]].y) || + !std::isfinite(input_->points[(*indices_)[iii]].z)) { + if (extract_removed_indices_) { + (*removed_indices_)[rii++] = (*indices_)[iii]; + } + continue; + } + indices[oii++] = (*indices_)[iii]; + } + } else { + // Attempt to get the field name's index + std::vector fields; + int distance_idx = pcl::getFieldIndex(filter_field_name_, fields); + if (distance_idx == -1) { + PCL_WARN( + "[pcl::%s::applyFilter] Unable to find field name in point type.\n", + getClassName().c_str()); + indices.clear(); + removed_indices_->clear(); + return; + } + + // Filter for non-finite entries and the specified field limits + for (int iii = 0; iii < static_cast(indices_->size()); + ++iii) // iii = input indices iterator + { + // Non-finite entries are always passed to removed indices + if ( + !std::isfinite(input_->points[(*indices_)[iii]].x) || + !std::isfinite(input_->points[(*indices_)[iii]].y) || + !std::isfinite(input_->points[(*indices_)[iii]].z)) { + if (extract_removed_indices_) { + (*removed_indices_)[rii++] = (*indices_)[iii]; + } + continue; + } + + // Get the field's value + const std::uint8_t * pt_data = + reinterpret_cast(&input_->points[(*indices_)[iii]]); + std::uint16_t field_value = 0; + memcpy(&field_value, pt_data + fields[distance_idx].offset, sizeof(std::uint16_t)); + + // Remove NAN/INF/-INF values. We expect passthrough to output clean valid data. + if (!std::isfinite(field_value)) { + if (extract_removed_indices_) { + (*removed_indices_)[rii++] = (*indices_)[iii]; + } + continue; + } + + // Outside of the field limits are passed to removed indices + if (!negative_ && (field_value < filter_limit_min_ || field_value > filter_limit_max_)) { + if (extract_removed_indices_) { + (*removed_indices_)[rii++] = (*indices_)[iii]; + } + continue; + } + + // Inside of the field limits are passed to removed indices if negative was set + if (negative_ && field_value >= filter_limit_min_ && field_value <= filter_limit_max_) { + if (extract_removed_indices_) { + (*removed_indices_)[rii++] = (*indices_)[iii]; + } + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = (*indices_)[iii]; + } + } + + // Resize the output arrays + indices.resize(oii); + removed_indices_->resize(rii); +} + +#define PCL_INSTANTIATE_PassThroughUInt16(T) template class PCL_EXPORTS pcl::PassThroughUInt16; + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif // POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp new file mode 100644 index 0000000000000..94cdc5e2259bf --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp @@ -0,0 +1,48 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ + +#include "pointcloud_preprocessor/filter.hpp" + +#include + +#include + +namespace pointcloud_preprocessor +{ +class PointcloudAccumulatorComponent : public pointcloud_preprocessor::Filter +{ +protected: + virtual void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); + + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +private: + double accumulation_time_sec_; + boost::circular_buffer pointcloud_buffer_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit PointcloudAccumulatorComponent(const rclcpp::NodeOptions & options); +}; +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp new file mode 100644 index 0000000000000..9294329eb6900 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -0,0 +1,93 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +using autoware_utils::LinearRing2d; +using autoware_utils::MultiPoint2d; +using autoware_utils::Point2d; + +namespace pointcloud_preprocessor +{ +class Lanelet2MapFilterComponent : public rclcpp::Node +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PointCloud2Ptr = sensor_msgs::msg::PointCloud2::SharedPtr; + using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; + +public: + explicit Lanelet2MapFilterComponent(const rclcpp::NodeOptions & options); + +private: + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr pointcloud_sub_; + rclcpp::Publisher::SharedPtr filtered_pointcloud_pub_; + + lanelet::LaneletMapPtr lanelet_map_ptr_; + lanelet::ConstLanelets road_lanelets_; + + float voxel_size_x_; + float voxel_size_y_; + + void pointcloudCallback(const PointCloud2ConstPtr msg); + + void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + + bool transformPointCloud( + const std::string & in_target_frame, const PointCloud2ConstPtr & in_cloud_ptr, + PointCloud2 * out_cloud_ptr); + + LinearRing2d getConvexHull(const pcl::PointCloud::Ptr & input_cloud); + + lanelet::ConstLanelets getIntersectedLanelets( + const LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets_); + + pcl::PointCloud getLaneFilteredPointCloud( + const lanelet::ConstLanelets & joint_lanelets, + const pcl::PointCloud::Ptr & cloud); + + bool pointWithinLanelets(const Point2d & point, const lanelet::ConstLanelets & joint_lanelets); + + /** \brief Parameter service callback result : needed to be hold */ + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /** \brief Parameter service callback */ + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); +}; + +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml b/sensing/pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml new file mode 100644 index 0000000000000..198c43d81dd0f --- /dev/null +++ b/sensing/pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py new file mode 100644 index 0000000000000..4b3fac1baa65f --- /dev/null +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -0,0 +1,119 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + ns = "pointcloud_preprocessor" + pkg = "pointcloud_preprocessor" + + # declare launch arguments + input_points_raw_list_param = DeclareLaunchArgument( + "input_points_raw_list", + default_value="['/points_raw']", + description="Input pointcloud topic_name list as a string_array. " + "To subscribe multiple topics, write as: \"['/points_raw0', '/points_raw1', ...]\"", + ) + + output_points_raw_param = DeclareLaunchArgument( + "output_points_raw", default_value="/points_raw/cropbox/filtered" + ) + + tf_output_frame_param = DeclareLaunchArgument("tf_output_frame", default_value="base_link") + + # set concat filter as a component + concat_component = ComposableNode( + package=pkg, + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_filter", + remappings=[("output", "points_raw/concatenated")], + parameters=[ + { + "input_topics": LaunchConfiguration("input_points_raw_list"), + "output_frame": LaunchConfiguration("tf_output_frame"), + "approximate_sync": True, + } + ], + ) + + # set crop box filter as a component + cropbox_component = ComposableNode( + package=pkg, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter", + remappings=[ + ( + "input", + PythonExpression( + [ + "'points_raw/concatenated' if len(", + LaunchConfiguration("input_points_raw_list"), + ") > 1 else 'input_points_raw0'", + ] + ), + ), + ("output", LaunchConfiguration("output_points_raw")), + ], + parameters=[ + { + "input_frame": LaunchConfiguration("tf_output_frame"), + "output_frame": LaunchConfiguration("tf_output_frame"), + "min_x": -200.0, + "max_x": 1000.0, + "min_y": -50.0, + "max_y": 50.0, + "min_z": -2.0, + "max_z": 3.0, + "negative": False, + } + ], + ) + + # set container to run all required components in the same process + container = ComposableNodeContainer( + name="pointcloud_preprocessor_container", + namespace=ns, + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[concat_component, cropbox_component], + output="screen", + ) + + # check the size of input_points_raw_list + log_info = LogInfo( + msg=PythonExpression( + [ + "'input_points_raw_list size = ' + str(len(", + LaunchConfiguration("input_points_raw_list"), + "))", + ] + ) + ) + + return launch.LaunchDescription( + [ + input_points_raw_list_param, + output_points_raw_param, + tf_output_frame_param, + container, + log_info, + ] + ) diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.xml b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.xml new file mode 100644 index 0000000000000..da149b6aa9937 --- /dev/null +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/launch/random_downsample_filter.launch.xml b/sensing/pointcloud_preprocessor/launch/random_downsample_filter.launch.xml new file mode 100644 index 0000000000000..96a7f3a01d9ca --- /dev/null +++ b/sensing/pointcloud_preprocessor/launch/random_downsample_filter.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml b/sensing/pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml new file mode 100644 index 0000000000000..92370c4f8039f --- /dev/null +++ b/sensing/pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml new file mode 100644 index 0000000000000..c91005f7c9228 --- /dev/null +++ b/sensing/pointcloud_preprocessor/package.xml @@ -0,0 +1,50 @@ + + + pointcloud_preprocessor + 0.1.0 + The ROS2 pointcloud_preprocessor package + + amc-nu + Yukihiro Saito + Apache License 2.0 + + Open Perception + Julius Kammerl + William Woodall + Paul Bovbel + Kentaro Wada + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_vehicle_msgs + autoware_debug_msgs + autoware_utils + diagnostic_updater + image_transport + lanelet2_extension + libopencv-dev + libpcl-all-dev + message_filters + nav_msgs + pcl_conversions + pcl_msgs + pcl_ros + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_pcl_extensions + vehicle_info_util + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp new file mode 100644 index 0000000000000..8ce39239de37e --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -0,0 +1,466 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ + * + */ + +#include "pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp" + +#include + +#include + +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pointcloud_preprocessor +{ +PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( + const rclcpp::NodeOptions & node_options) +: Node("point_cloud_concatenator_component", node_options) +{ + // Set parameters + { + output_frame_ = static_cast(declare_parameter("output_frame", "")); + if (output_frame_.empty()) { + RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!"); + return; + } + declare_parameter("input_topics", std::vector()); + input_topics_ = get_parameter("input_topics").as_string_array(); + if (input_topics_.empty()) { + RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!"); + return; + } + if (input_topics_.size() == 1) { + RCLCPP_ERROR(get_logger(), "Only one topic given. Need at least two topics to continue."); + return; + } + + // Optional parameters + maximum_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); + timeout_sec_ = static_cast(declare_parameter("timeout_sec", 0.1)); + + input_offset_ = declare_parameter("input_offset", std::vector{}); + if (!input_offset_.empty() && input_topics_.size() != input_offset_.size()) { + RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets."); + return; + } + } + + // Initialize not_subscribed_topic_names_ + { + for (const std::string & e : input_topics_) { + not_subscribed_topic_names_.insert(e); + } + } + + // Initialize offset map + { + for (size_t i = 0; i < input_offset_.size(); ++i) { + offset_map_[input_topics_[i]] = input_offset_[i]; + } + } + + // Publishers + { + pub_output_ = this->create_publisher( + "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + } + + // Subscribers + { + RCLCPP_INFO_STREAM( + get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:"); + for (auto & input_topic : input_topics_) { + RCLCPP_INFO_STREAM(get_logger(), " - " << input_topic); + } + + // Subscribe to the filters + filters_.resize(input_topics_.size()); + + // First input_topics_.size () filters are valid + for (size_t d = 0; d < input_topics_.size(); ++d) { + cloud_stdmap_.insert(std::make_pair(input_topics_[d], nullptr)); + cloud_stdmap_tmp_ = cloud_stdmap_; + + // CAN'T use auto type here. + std::function cb = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::cloud_callback, this, + std::placeholders::_1, input_topics_[d]); + + filters_[d].reset(); + filters_[d] = this->create_subscription( + input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); + } + auto twist_cb = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); + sub_twist_ = this->create_subscription( + "/vehicle/status/velocity_status", rclcpp::QoS{100}, twist_cb); + } + + // Set timer + { + auto cb = std::bind(&PointCloudConcatenateDataSynchronizerComponent::timer_callback, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + timer_ = std::make_shared>( + get_clock(), period, std::move(cb), get_node_base_interface()->get_context()); + get_node_timers_interface()->add_timer(timer_, nullptr); + } + + // Diagnostic Updater + { + updater_.setHardwareID("concatenate_data_checker"); + updater_.add( + "concat_status", this, &PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void PointCloudConcatenateDataSynchronizerComponent::transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out) +{ + // Transform the point clouds into the specified output frame + if (output_frame_ != in->header.frame_id) { + // TODO(YamatoAndo): use TF2 + if (!pcl_ros::transformPointCloud(output_frame_, *in, *out, *tf2_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), + "[transformPointCloud] Error converting first input dataset from %s to %s.", + in->header.frame_id.c_str(), output_frame_.c_str()); + return; + } + } else { + out = std::make_shared(*in); + } +} + +void PointCloudConcatenateDataSynchronizerComponent::combineClouds( + const PointCloud2::ConstSharedPtr & in1, const PointCloud2::ConstSharedPtr & in2, + PointCloud2::SharedPtr & out) +{ + if (twist_ptr_queue_.empty()) { + pcl::concatenatePointCloud(*in1, *in2, *out); + out->header.stamp = std::min(rclcpp::Time(in1->header.stamp), rclcpp::Time(in2->header.stamp)); + return; + } + + const auto old_stamp = std::min(rclcpp::Time(in1->header.stamp), rclcpp::Time(in2->header.stamp)); + auto old_twist_ptr_it = std::lower_bound( + std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), old_stamp, + [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { + return rclcpp::Time(x_ptr->header.stamp) < t; + }); + old_twist_ptr_it = + old_twist_ptr_it == twist_ptr_queue_.end() ? (twist_ptr_queue_.end() - 1) : old_twist_ptr_it; + + const auto new_stamp = std::max(rclcpp::Time(in1->header.stamp), rclcpp::Time(in2->header.stamp)); + auto new_twist_ptr_it = std::lower_bound( + std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), new_stamp, + [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { + return rclcpp::Time(x_ptr->header.stamp) < t; + }); + new_twist_ptr_it = + new_twist_ptr_it == twist_ptr_queue_.end() ? (twist_ptr_queue_.end() - 1) : new_twist_ptr_it; + + auto prev_time = old_stamp; + double x = 0.0; + double y = 0.0; + double yaw = 0.0; + for (auto twist_ptr_it = old_twist_ptr_it; twist_ptr_it != new_twist_ptr_it + 1; ++twist_ptr_it) { + const double dt = + (twist_ptr_it != new_twist_ptr_it) + ? (rclcpp::Time((*twist_ptr_it)->header.stamp) - rclcpp::Time(prev_time)).seconds() + : (rclcpp::Time(new_stamp) - rclcpp::Time(prev_time)).seconds(); + + if (std::fabs(dt) > 0.1) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "Time difference is too large. Cloud not interpolate. Please confirm twist topic and " + "timestamp"); + break; + } + + const double dis = (*twist_ptr_it)->twist.linear.x * dt; + yaw += (*twist_ptr_it)->twist.angular.z * dt; + x += dis * std::cos(yaw); + y += dis * std::sin(yaw); + prev_time = (*twist_ptr_it)->header.stamp; + } + Eigen::AngleAxisf rotation_x(0, Eigen::Vector3f::UnitX()); + Eigen::AngleAxisf rotation_y(0, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf rotation_z(yaw, Eigen::Vector3f::UnitZ()); + Eigen::Translation3f translation(x, y, 0); + Eigen::Matrix4f rotation_matrix = (translation * rotation_z * rotation_y * rotation_x).matrix(); + + // TODO(YamatoAndo): if output_frame_ is not base_link, we must transform + + if (rclcpp::Time(in1->header.stamp) > rclcpp::Time(in2->header.stamp)) { + sensor_msgs::msg::PointCloud2::SharedPtr in1_t(new sensor_msgs::msg::PointCloud2()); + pcl_ros::transformPointCloud(rotation_matrix, *in1, *in1_t); + pcl::concatenatePointCloud(*in1_t, *in2, *out); + out->header.stamp = in2->header.stamp; + } else { + sensor_msgs::msg::PointCloud2::SharedPtr in2_t(new sensor_msgs::msg::PointCloud2()); + pcl_ros::transformPointCloud(rotation_matrix, *in2, *in2_t); + pcl::concatenatePointCloud(*in1, *in2_t, *out); + out->header.stamp = in1->header.stamp; + } +} + +void PointCloudConcatenateDataSynchronizerComponent::publish() +{ + sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr_ = nullptr; + not_subscribed_topic_names_.clear(); + + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + transformPointCloud(e.second, transformed_cloud_ptr); + if (concat_cloud_ptr_ == nullptr) { + concat_cloud_ptr_ = transformed_cloud_ptr; + } else { + PointCloudConcatenateDataSynchronizerComponent::combineClouds( + concat_cloud_ptr_, transformed_cloud_ptr, concat_cloud_ptr_); + } + + } else { + not_subscribed_topic_names_.insert(e.first); + } + } + + if (concat_cloud_ptr_) { + auto output = std::make_unique(*concat_cloud_ptr_); + pub_output_->publish(std::move(output)); + } else { + RCLCPP_WARN(this->get_logger(), "concat_cloud_ptr_ is nullptr, skipping pointcloud publish."); + } + + updater_.force_update(); + + cloud_stdmap_ = cloud_stdmap_tmp_; + std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { + e.second = nullptr; + }); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +void PointCloudConcatenateDataSynchronizerComponent::removeRADTFields( + const sensor_msgs::msg::PointCloud2 & input_cloud, sensor_msgs::msg::PointCloud2 & output_cloud) +{ + bool has_intensity = std::any_of( + input_cloud.fields.begin(), input_cloud.fields.end(), + [](auto & field) { return field.name == "intensity"; }); + + if (input_cloud.fields.size() == 3 || (input_cloud.fields.size() == 4 && has_intensity)) { + output_cloud = input_cloud; + } else if (has_intensity) { + pcl::PointCloud tmp_cloud; + pcl::fromROSMsg(input_cloud, tmp_cloud); + pcl::toROSMsg(tmp_cloud, output_cloud); + output_cloud.header = input_cloud.header; + } else { + pcl::PointCloud tmp_cloud; + pcl::fromROSMsg(input_cloud, tmp_cloud); + pcl::toROSMsg(tmp_cloud, output_cloud); + output_cloud.header = input_cloud.header; + } +} + +void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + +void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) +{ + std::lock_guard lock(mutex_); + + sensor_msgs::msg::PointCloud2 xyz_cloud; + removeRADTFields(*input_ptr, xyz_cloud); + sensor_msgs::msg::PointCloud2::ConstSharedPtr xyz_input_ptr( + new sensor_msgs::msg::PointCloud2(xyz_cloud)); + + const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); + const bool is_already_subscribed_tmp = std::any_of( + std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), + [](const auto & e) { return e.second != nullptr; }); + + if (is_already_subscribed_this) { + cloud_stdmap_tmp_[topic_name] = xyz_input_ptr; + + if (!is_already_subscribed_tmp) { + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } else { + cloud_stdmap_[topic_name] = xyz_input_ptr; + + const bool is_subscribed_all = std::all_of( + std::begin(cloud_stdmap_), std::end(cloud_stdmap_), + [](const auto & e) { return e.second != nullptr; }); + + if (is_subscribed_all) { + for (const auto & e : cloud_stdmap_tmp_) { + if (e.second != nullptr) { + cloud_stdmap_[e.first] = e.second; + } + } + std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { + e.second = nullptr; + }); + + timer_->cancel(); + publish(); + } else if (offset_map_.size() > 0) { + timer_->cancel(); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_ - offset_map_[topic_name])); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } +} + +void PointCloudConcatenateDataSynchronizerComponent::timer_callback() +{ + using std::chrono_literals::operator""ms; + timer_->cancel(); + if (mutex_.try_lock()) { + publish(); + mutex_.unlock(); + } else { + try { + std::chrono::nanoseconds period = 10ms; + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } +} + +void PointCloudConcatenateDataSynchronizerComponent::twist_callback( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input) +{ + // if rosbag restart, clear buffer + if (!twist_ptr_queue_.empty()) { + if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { + twist_ptr_queue_.clear(); + } + } + + // pop old data + while (!twist_ptr_queue_.empty()) { + if ( + rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > + rclcpp::Time(input->header.stamp)) { + break; + } + twist_ptr_queue_.pop_front(); + } + + auto twist_ptr = std::make_shared(); + twist_ptr->header.stamp = input->header.stamp; + twist_ptr->twist.linear.x = input->longitudinal_velocity; + twist_ptr->twist.linear.y = input->lateral_velocity; + twist_ptr->twist.angular.z = input->heading_rate; + twist_ptr_queue_.push_back(twist_ptr); +} + +void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + for (const std::string & e : input_topics_) { + const std::string subscribe_status = not_subscribed_topic_names_.count(e) ? "NG" : "OK"; + stat.add(e, subscribe_status); + } + + const int8_t level = not_subscribed_topic_names_.empty() + ? diagnostic_msgs::msg::DiagnosticStatus::OK + : diagnostic_msgs::msg::DiagnosticStatus::WARN; + const std::string message = not_subscribed_topic_names_.empty() + ? "Concatenate all topics" + : "Some topics are not concatenated"; + stat.summary(level, message); +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent) diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp new file mode 100644 index 0000000000000..627257fdd15c7 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -0,0 +1,231 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: cropbox.cpp + * + */ + +#include "pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp" + +#include + +namespace pointcloud_preprocessor +{ +CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & options) +: Filter("CropBoxFilter", options) +{ + // set initial parameters + { + Eigen::Vector4f new_min_point = Eigen::Vector4f::Zero(); + new_min_point(0) = static_cast(declare_parameter("min_x", -1.0)); + new_min_point(1) = static_cast(declare_parameter("min_y", -1.0)); + new_min_point(2) = static_cast(declare_parameter("min_z", -1.0)); + impl_.setMin(new_min_point); + + Eigen::Vector4f new_max_point = Eigen::Vector4f::Zero(); + new_max_point(0) = static_cast(declare_parameter("max_x", 1.0)); + new_max_point(1) = static_cast(declare_parameter("max_y", 1.0)); + new_max_point(2) = static_cast(declare_parameter("max_z", 1.0)); + impl_.setMax(new_max_point); + + impl_.setKeepOrganized(static_cast(declare_parameter("keep_organized", false))); + impl_.setNegative(static_cast(declare_parameter("negative", false))); + } + + // set additional publishers + { + crop_box_polygon_pub_ = + this->create_publisher("~/crop_box_polygon", 10); + } + + // set parameter service callback + { + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&CropBoxFilterComponent::paramCallback, this, _1)); + } +} + +void CropBoxFilterComponent::filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); + + publishCropBoxPolygon(); +} + +void CropBoxFilterComponent::publishCropBoxPolygon() +{ + auto generatePoint = [](double x, double y, double z) { + geometry_msgs::msg::Point32 point; + point.x = x; + point.y = y; + point.z = z; + return point; + }; + + const double x1 = impl_.getMax()(0); + const double x2 = impl_.getMin()(0); + const double x3 = impl_.getMin()(0); + const double x4 = impl_.getMax()(0); + + const double y1 = impl_.getMax()(1); + const double y2 = impl_.getMax()(1); + const double y3 = impl_.getMin()(1); + const double y4 = impl_.getMin()(1); + + const double z1 = impl_.getMin()(2); + const double z2 = impl_.getMax()(2); + + geometry_msgs::msg::PolygonStamped polygon_msg; + polygon_msg.header.frame_id = tf_input_frame_; + polygon_msg.header.stamp = get_clock()->now(); + polygon_msg.polygon.points.push_back(generatePoint(x1, y1, z1)); + polygon_msg.polygon.points.push_back(generatePoint(x2, y2, z1)); + polygon_msg.polygon.points.push_back(generatePoint(x3, y3, z1)); + polygon_msg.polygon.points.push_back(generatePoint(x4, y4, z1)); + polygon_msg.polygon.points.push_back(generatePoint(x1, y1, z1)); + + polygon_msg.polygon.points.push_back(generatePoint(x1, y1, z2)); + + polygon_msg.polygon.points.push_back(generatePoint(x2, y2, z2)); + polygon_msg.polygon.points.push_back(generatePoint(x2, y2, z1)); + polygon_msg.polygon.points.push_back(generatePoint(x2, y2, z2)); + + polygon_msg.polygon.points.push_back(generatePoint(x3, y3, z2)); + polygon_msg.polygon.points.push_back(generatePoint(x3, y3, z1)); + polygon_msg.polygon.points.push_back(generatePoint(x3, y3, z2)); + + polygon_msg.polygon.points.push_back(generatePoint(x4, y4, z2)); + polygon_msg.polygon.points.push_back(generatePoint(x4, y4, z1)); + polygon_msg.polygon.points.push_back(generatePoint(x4, y4, z2)); + + polygon_msg.polygon.points.push_back(generatePoint(x1, y1, z2)); + + crop_box_polygon_pub_->publish(polygon_msg); +} + +rcl_interfaces::msg::SetParametersResult CropBoxFilterComponent::paramCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + Eigen::Vector4f min_point, max_point; + min_point = impl_.getMin(); + max_point = impl_.getMax(); + + Eigen::Vector4f new_min_point = Eigen::Vector4f::Zero(); + Eigen::Vector4f new_max_point = Eigen::Vector4f::Zero(); + + // Check the current values for minimum point + if ( + get_param(p, "min_x", new_min_point(0)) && get_param(p, "min_y", new_min_point(1)) && + get_param(p, "min_z", new_min_point(2))) { + if (min_point != new_min_point) { + RCLCPP_DEBUG( + get_logger(), "[%s::paramCallback] Setting the minimum point to: %f %f %f.", get_name(), + new_min_point(0), new_min_point(1), new_min_point(2)); + // Set the filter min point if different + impl_.setMin(new_min_point); + } + } + + // Check the current values for the maximum point + if ( + get_param(p, "max_x", new_max_point(0)) && get_param(p, "max_y", new_max_point(1)) && + get_param(p, "max_z", new_max_point(2))) { + if (max_point != new_max_point) { + RCLCPP_DEBUG( + get_logger(), "[%s::paramCallback] Setting the maximum point to: %f %f %f.", get_name(), + new_max_point(0), new_max_point(1), new_max_point(2)); + // Set the filter max point if different + impl_.setMax(new_max_point); + } + } + + // Check the current value for keep_organized + bool keep_organized; + if (get_param(p, "keep_organized", keep_organized)) { + if (impl_.getKeepOrganized() != keep_organized) { + RCLCPP_DEBUG( + get_logger(), "[%s::paramCallback] Setting the filter keep_organized value to: %s.", + get_name(), keep_organized ? "true" : "false"); + // Call the virtual method in the child + impl_.setKeepOrganized(keep_organized); + } + } + + // Check the current value for the negative flag + bool negative; + if (get_param(p, "negative", negative)) { + if (impl_.getNegative() != negative) { + RCLCPP_DEBUG( + get_logger(), "[%s::paramCallback] Setting the filter negative flag to: %s.", get_name(), + negative ? "true" : "false"); + // Call the virtual method in the child + impl_.setNegative(negative); + } + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} + +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::CropBoxFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp new file mode 100644 index 0000000000000..1a73d6c301b9b --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -0,0 +1,203 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" + +#include +#include +#include + +namespace pointcloud_preprocessor +{ +/** @brief Constructor. */ +DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOptions & options) +: Node("distortion_corrector_node", options) +{ + // Parameter + time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); + + // Publisher + undistorted_points_pub_ = + this->create_publisher("~/output/pointcloud", rclcpp::SensorDataQoS()); + + // Subscriber + velocity_report_sub_ = this->create_subscription( + "~/input/velocity_report", 10, + std::bind(&DistortionCorrectorComponent::onVelocityReport, this, std::placeholders::_1)); + input_points_sub_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&DistortionCorrectorComponent::onPointCloud, this, std::placeholders::_1)); +} + +void DistortionCorrectorComponent::onVelocityReport( + const VelocityReport::ConstSharedPtr velocity_report_msg) +{ + velocity_report_queue_.push_back(*velocity_report_msg); + + while (!velocity_report_queue_.empty()) { + // for replay rosbag + if ( + rclcpp::Time(velocity_report_queue_.front().header.stamp) > + rclcpp::Time(velocity_report_msg->header.stamp)) { + velocity_report_queue_.pop_front(); + } else if ( // NOLINT + rclcpp::Time(velocity_report_queue_.front().header.stamp) < + rclcpp::Time(velocity_report_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0)) { + velocity_report_queue_.pop_front(); + } + break; + } +} + +void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_msg) +{ + const auto points_sub_count = undistorted_points_pub_->get_subscription_count() + + undistorted_points_pub_->get_intra_process_subscription_count(); + + if (points_sub_count < 1) { + return; + } + + tf2::Transform tf2_base_link_to_sensor{}; + getTransform(points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor); + + undistortPointCloud(velocity_report_queue_, tf2_base_link_to_sensor, *points_msg); + + if (points_sub_count > 0) { + undistorted_points_pub_->publish(std::move(points_msg)); + } +} + +bool DistortionCorrectorComponent::getTransform( + const std::string & target_frame, const std::string & source_frame, + tf2::Transform * tf2_transform_ptr) +{ + if (target_frame == source_frame) { + tf2_transform_ptr->setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_transform_ptr->setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + return true; + } + + try { + const auto transform_msg = + tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + tf2::convert(transform_msg.transform, *tf2_transform_ptr); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + RCLCPP_ERROR( + get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + + tf2_transform_ptr->setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + tf2_transform_ptr->setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); + return false; + } + return true; +} + +bool DistortionCorrectorComponent::undistortPointCloud( + const std::deque & velocity_report_queue_, + const tf2::Transform & tf2_base_link_to_sensor, PointCloud2 & points) +{ + if (points.data.empty() || velocity_report_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 10000 /* ms */, + "input_pointcloud->points or velocity_report_queue_ is empty."); + return false; + } + + auto time_stamp_field_it = std::find_if( + std::cbegin(points.fields), std::cend(points.fields), + [this](const sensor_msgs::msg::PointField & field) { + return field.name == time_stamp_field_name_; + }); + if (time_stamp_field_it == points.fields.cend()) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 10000 /* ms */, + "Required field time stamp doesn't exist in the point cloud."); + return false; + } + + sensor_msgs::PointCloud2Iterator it_x(points, "x"); + sensor_msgs::PointCloud2Iterator it_y(points, "y"); + sensor_msgs::PointCloud2Iterator it_z(points, "z"); + sensor_msgs::PointCloud2ConstIterator it_time_stamp(points, time_stamp_field_name_); + + float theta{0.0f}; + float x{0.0f}; + float y{0.0f}; + double prev_time_stamp_sec{*it_time_stamp}; + const double first_point_time_stamp_sec{*it_time_stamp}; + + auto velocity_report_it = std::lower_bound( + std::begin(velocity_report_queue_), std::end(velocity_report_queue_), + first_point_time_stamp_sec, [](const VelocityReport & x, const double t) { + return rclcpp::Time(x.header.stamp).seconds() < t; + }); + velocity_report_it = velocity_report_it == std::end(velocity_report_queue_) + ? std::end(velocity_report_queue_) - 1 + : velocity_report_it; + + const tf2::Transform tf2_base_link_to_sensor_inv{tf2_base_link_to_sensor.inverse()}; + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { + for (; + (velocity_report_it != std::end(velocity_report_queue_) - 1 && + *it_time_stamp > rclcpp::Time(velocity_report_it->header.stamp).seconds()); + ++velocity_report_it) { + } + + float v{static_cast(velocity_report_it->longitudinal_velocity)}; + float w{static_cast(velocity_report_it->heading_rate)}; + + if (std::abs(*it_time_stamp - rclcpp::Time(velocity_report_it->header.stamp).seconds()) > 0.1) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 10000 /* ms */, + "velocity_report time_stamp is too late. Cloud not interpolate."); + v = 0.0f; + w = 0.0f; + } + + const float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); + + const tf2::Vector3 sensorTF_point{*it_x, *it_y, *it_z}; + + const tf2::Vector3 base_linkTF_point{tf2_base_link_to_sensor_inv * sensorTF_point}; + + theta += w * time_offset; + tf2::Quaternion baselink_quat{}; + baselink_quat.setRPY(0.0, 0.0, theta); + const float dis = v * time_offset; + x += dis * std::cos(theta); + y += dis * std::sin(theta); + + tf2::Transform baselinkTF_odom{}; + baselinkTF_odom.setOrigin(tf2::Vector3(x, y, 0.0)); + baselinkTF_odom.setRotation(baselink_quat); + + const tf2::Vector3 base_linkTF_trans_point{baselinkTF_odom * base_linkTF_point}; + + const tf2::Vector3 sensorTF_trans_point{tf2_base_link_to_sensor * base_linkTF_trans_point}; + + *it_x = sensorTF_trans_point.getX(); + *it_y = sensorTF_trans_point.getY(); + *it_z = sensorTF_trans_point.getZ(); + + prev_time_stamp_sec = *it_time_stamp; + } + return true; +} + +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DistortionCorrectorComponent) diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp new file mode 100644 index 0000000000000..26ce9747a73b6 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp @@ -0,0 +1,120 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#include "pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp" + +#include +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +ApproximateDownsampleFilterComponent::ApproximateDownsampleFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("ApproximateDownsampleFilter", options) +{ + { + voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); + voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); + voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); + } + + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&ApproximateDownsampleFilterComponent::paramCallback, this, _1)); +} + +void ApproximateDownsampleFilterComponent::filter( + const PointCloud2ConstPtr & input, const IndicesPtr & /*indices*/, PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + pcl_output->points.reserve(pcl_input->points.size()); + pcl::VoxelGridNearestCentroid filter; + filter.setInputCloud(pcl_input); + filter.setLeafSize(voxel_size_x_, voxel_size_y_, voxel_size_z_); + filter.filter(*pcl_output); + + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; +} + +rcl_interfaces::msg::SetParametersResult ApproximateDownsampleFilterComponent::paramCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (get_param(p, "voxel_size_x", voxel_size_x_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_x_); + } + + if (get_param(p, "voxel_size_y", voxel_size_y_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_y_); + } + + if (get_param(p, "voxel_size_z", voxel_size_z_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_z_); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::ApproximateDownsampleFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp new file mode 100644 index 0000000000000..410c1a1171a0b --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp @@ -0,0 +1,105 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp" + +#include + +namespace pointcloud_preprocessor +{ +RandomDownsampleFilterComponent::RandomDownsampleFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("RandomDownsampleFilter", options) +{ + // set initial parameters + { + sample_num_ = static_cast(declare_parameter("sample_num", 1500)); + } + + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RandomDownsampleFilterComponent::paramCallback, this, _1)); +} + +void RandomDownsampleFilterComponent::filter( + const PointCloud2ConstPtr & input, const IndicesPtr & /*indices*/, PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + pcl_output->points.reserve(pcl_input->points.size()); + pcl::RandomSample filter; + filter.setInputCloud(pcl_input); + // filter.setSaveLeafLayout(true); + filter.setSample(sample_num_); + filter.filter(*pcl_output); + + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; +} + +rcl_interfaces::msg::SetParametersResult RandomDownsampleFilterComponent::paramCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (get_param(p, "sample_num", sample_num_)) { + RCLCPP_DEBUG(get_logger(), "Setting new sample num to: %zu.", sample_num_); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} + +} // namespace pointcloud_preprocessor +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::RandomDownsampleFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp new file mode 100644 index 0000000000000..cbdc8ba0e85d8 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -0,0 +1,119 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#include "pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp" + +#include +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("VoxelGridDownsampleFilter", options) +{ + // set initial parameters + { + voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); + voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); + voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); + } + + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&VoxelGridDownsampleFilterComponent::paramCallback, this, _1)); +} + +void VoxelGridDownsampleFilterComponent::filter( + const PointCloud2ConstPtr & input, const IndicesPtr & /*indices*/, PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + pcl_output->points.reserve(pcl_input->points.size()); + pcl::VoxelGrid filter; + filter.setInputCloud(pcl_input); + // filter.setSaveLeafLayout(true); + filter.setLeafSize(voxel_size_x_, voxel_size_y_, voxel_size_z_); + filter.filter(*pcl_output); + + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; +} + +rcl_interfaces::msg::SetParametersResult VoxelGridDownsampleFilterComponent::paramCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (get_param(p, "voxel_size_x", voxel_size_x_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_x_); + } + if (get_param(p, "voxel_size_y", voxel_size_y_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_y_); + } + if (get_param(p, "voxel_size_z", voxel_size_z_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_z_); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::VoxelGridDownsampleFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp new file mode 100644 index 0000000000000..323181e94a731 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -0,0 +1,302 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: filter.cpp 35876 2011-02-09 01:04:36Z rusu $ + * + */ + +#include "pointcloud_preprocessor/filter.hpp" + +#include + +#include + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +pointcloud_preprocessor::Filter::Filter( + const std::string & filter_name, const rclcpp::NodeOptions & options) +: Node(filter_name, options), filter_field_name_(filter_name) +{ + // Set parameters (moved from NodeletLazy onInit) + { + tf_input_frame_ = static_cast(declare_parameter("input_frame", "")); + tf_output_frame_ = static_cast(declare_parameter("output_frame", "")); + max_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); + + // ---[ Optional parameters + use_indices_ = static_cast(declare_parameter("use_indices", false)); + latched_indices_ = static_cast(declare_parameter("latched_indices", false)); + approximate_sync_ = static_cast(declare_parameter("approximate_sync", false)); + + RCLCPP_INFO_STREAM( + this->get_logger(), + "Filter (as Component) successfully created with the following parameters:" + << std::endl + << " - approximate_sync : " << (approximate_sync_ ? "true" : "false") << std::endl + << " - use_indices : " << (use_indices_ ? "true" : "false") << std::endl + << " - latched_indices : " << (latched_indices_ ? "true" : "false") << std::endl + << " - max_queue_size : " << max_queue_size_); + } + + // Set publisher + { + pub_output_ = this->create_publisher( + "output", rclcpp::SensorDataQoS().keep_last(max_queue_size_)); + } + + subscribe(); + + // Set tf_listener, tf_buffer. + setupTF(); + + // Set parameter service callback + set_param_res_filter_ = this->add_on_set_parameters_callback( + std::bind(&Filter::filterParamCallback, this, std::placeholders::_1)); + + RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created."); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void pointcloud_preprocessor::Filter::setupTF() +{ + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void pointcloud_preprocessor::Filter::subscribe() +{ + if (use_indices_) { + // Subscribe to the input using a filter + sub_input_filter_.subscribe( + this, "input", rclcpp::SensorDataQoS().keep_last(max_queue_size_).get_rmw_qos_profile()); + sub_indices_filter_.subscribe( + this, "indices", rclcpp::SensorDataQoS().keep_last(max_queue_size_).get_rmw_qos_profile()); + + if (approximate_sync_) { + sync_input_indices_a_ = std::make_shared(max_queue_size_); + sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback(std::bind( + &Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2)); + } else { + sync_input_indices_e_ = std::make_shared(max_queue_size_); + sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_e_->registerCallback(std::bind( + &Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2)); + } + } else { + // Subscribe in an old fashion to input only (no filters) + // CAN'T use auto-type here. + std::function cb = std::bind( + &Filter::input_indices_callback, this, std::placeholders::_1, PointIndicesConstPtr()); + sub_input_ = create_subscription( + "input", rclcpp::SensorDataQoS().keep_last(max_queue_size_), cb); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void pointcloud_preprocessor::Filter::unsubscribe() +{ + if (use_indices_) { + sub_input_filter_.unsubscribe(); + sub_indices_filter_.unsubscribe(); + if (approximate_sync_) { + sync_input_indices_a_.reset(); + } else { + sync_input_indices_e_.reset(); + } + } else { + sub_input_.reset(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void pointcloud_preprocessor::Filter::computePublish( + const PointCloud2ConstPtr & input, const IndicesPtr & indices) +{ + PointCloud2 output; + // Call the virtual method in the child + filter(input, indices, output); + + auto cloud_tf = std::make_unique(output); // set the output by default + // Check whether the user has given a different output TF frame + if (!tf_output_frame_.empty() && output.header.frame_id != tf_output_frame_) { + RCLCPP_DEBUG( + this->get_logger(), "[computePublish] Transforming output dataset from %s to %s.", + output.header.frame_id.c_str(), tf_output_frame_.c_str()); + // Convert the cloud into the different frame + PointCloud2 cloud_transformed; + if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, *tf_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), "[computePublish] Error converting output dataset from %s to %s.", + output.header.frame_id.c_str(), tf_output_frame_.c_str()); + return; + } + cloud_tf.reset(new PointCloud2(cloud_transformed)); + } + if (tf_output_frame_.empty() && output.header.frame_id != tf_input_orig_frame_) { + // no tf_output_frame given, transform the dataset to its original frame + RCLCPP_DEBUG( + this->get_logger(), "[computePublish] Transforming output dataset from %s back to %s.", + output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); + // Convert the cloud into the different frame + PointCloud2 cloud_transformed; + if (!pcl_ros::transformPointCloud( + tf_input_orig_frame_, output, cloud_transformed, *tf_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), "[computePublish] Error converting output dataset from %s back to %s.", + output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); + return; + } + cloud_tf.reset(new PointCloud2(cloud_transformed)); + } + + // Copy timestamp to keep it + cloud_tf->header.stamp = input->header.stamp; + + // Publish a boost shared ptr + pub_output_->publish(std::move(cloud_tf)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult pointcloud_preprocessor::Filter::filterParamCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (get_param(p, "input_frame", tf_input_frame_)) { + RCLCPP_DEBUG(get_logger(), "Setting the input TF frame to: %s.", tf_input_frame_.c_str()); + } + if (get_param(p, "output_frame", tf_output_frame_)) { + RCLCPP_DEBUG(get_logger(), "Setting the output TF frame to: %s.", tf_output_frame_.c_str()); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void pointcloud_preprocessor::Filter::input_indices_callback( + const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices) +{ + // If cloud is given, check if it's valid + if (!isValid(cloud)) { + RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid input!"); + return; + } + // If indices are given, check if they are valid + if (indices && !isValid(indices)) { + RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid indices!"); + return; + } + + /// DEBUG + if (indices) { + RCLCPP_DEBUG( + this->get_logger(), + "[input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %f, and frame %s on input topic received.\n" + " - PointIndices with %zu values, stamp %f, and frame %s on indices topic received.", + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + rclcpp::Time(cloud->header.stamp).seconds(), cloud->header.frame_id.c_str(), + indices->indices.size(), rclcpp::Time(indices->header.stamp).seconds(), + indices->header.frame_id.c_str()); + } else { + RCLCPP_DEBUG( + this->get_logger(), + "[input_indices_callback] PointCloud with %d data points and frame %s on input topic " + "received.", + cloud->width * cloud->height, cloud->header.frame_id.c_str()); + } + /// + + // Check whether the user has given a different input TF frame + tf_input_orig_frame_ = cloud->header.frame_id; + PointCloud2ConstPtr cloud_tf; + if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) { + RCLCPP_DEBUG( + this->get_logger(), "[input_indices_callback] Transforming input dataset from %s to %s.", + cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); + // Save the original frame ID + // Convert the cloud into the different frame + PointCloud2 cloud_transformed; + + if (!tf_buffer_->canTransform( + tf_input_frame_, cloud->header.frame_id, this->now(), + rclcpp::Duration::from_seconds(1.0))) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "[input_indices_callback] timeout tf: " << cloud->header.frame_id + << "->" << tf_input_frame_); + return; + } + + if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, *tf_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), + "[input_indices_callback] Error converting input dataset from %s to %s.", + cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); + return; + } + cloud_tf = std::make_shared(cloud_transformed); + } else { + cloud_tf = cloud; + } + // Need setInputCloud () here because we have to extract x/y/z + IndicesPtr vindices; + if (indices) { + vindices.reset(new std::vector(indices->indices)); + } + + computePublish(cloud_tf, vindices); +} diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp new file mode 100644 index 0000000000000..de1af0318dac7 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -0,0 +1,289 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp" + +#include + +#include +#include +#include + +#include +#include +#include + +namespace pointcloud_preprocessor +{ +using diagnostic_msgs::msg::DiagnosticStatus; + +DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("DualReturnOutlierFilter", options) +{ + // set initial parameters + { + vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); + max_azimuth_diff_ = static_cast(declare_parameter("max_azimuth_diff", 50.0)); + weak_first_distance_ratio_ = + static_cast(declare_parameter("weak_first_distance_ratio", 1.004)); + general_distance_ratio_ = + static_cast(declare_parameter("general_distance_ratio", 1.03)); + + weak_first_local_noise_threshold_ = + static_cast(declare_parameter("weak_first_local_noise_threshold", 10)); + visibility_threshold_ = static_cast(declare_parameter("visibility_threshold", 0.5)); + } + updater_.setHardwareID("dual_return_outlier_filter"); + updater_.add( + std::string(this->get_namespace()) + ": visibility_validation", this, + &DualReturnOutlierFilterComponent::onVisibilityChecker); + updater_.setPeriod(0.1); + + image_pub_ = + image_transport::create_publisher(this, "/dual_return_outlier_filter/frequency_image"); + visibility_pub_ = create_publisher( + "/dual_return_outlier_filter/visibility", rclcpp::SensorDataQoS()); + noise_cloud_pub_ = create_publisher( + "/dual_return_outlier_filter/pointcloud_noise", rclcpp::SensorDataQoS()); + + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&DualReturnOutlierFilterComponent::paramCallback, this, _1)); +} + +void DualReturnOutlierFilterComponent::onVisibilityChecker(DiagnosticStatusWrapper & stat) +{ + // Add values + stat.add("value", std::to_string(visibility_)); + + // Judge level + const auto level = + visibility_ > visibility_threshold_ ? DiagnosticStatus::OK : DiagnosticStatus::WARN; + + // Set message + std::string msg; + if (level == DiagnosticStatus::OK) { + msg = "OK"; + } else if (level == DiagnosticStatus::WARN) { + msg = "low visibility in dual outlier filter"; + } + stat.summary(level, msg); +} + +void DualReturnOutlierFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + pcl::PointCloud::Ptr pcl_input( + new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + if (pcl_input->points.empty()) { + return; + } + + uint32_t vertical_bins = vertical_bins_; + uint32_t horizontal_bins = 36; + + pcl::PointCloud::Ptr pcl_output( + new pcl::PointCloud); + pcl_output->points.reserve(pcl_input->points.size()); + + std::vector> pcl_input_ring_array; + std::vector> weak_first_pcl_input_ring_array; + + pcl::PointCloud::Ptr noise_output( + new pcl::PointCloud); + noise_output->points.reserve(pcl_input->points.size()); + pcl_input_ring_array.resize( + vertical_bins); // TODO(davidw): this is for Pandar 40 only, make dynamic + weak_first_pcl_input_ring_array.resize(vertical_bins); + + // Split into 36 x 10 degree bins x 40 lines (TODO: change to dynamic) + for (const auto & p : pcl_input->points) { + if (p.return_type == ReturnType::DUAL_WEAK_FIRST) { + weak_first_pcl_input_ring_array.at(p.ring).push_back(p); + } else { + pcl_input_ring_array.at(p.ring).push_back(p); + } + } + + float max_azimuth_diff = max_azimuth_diff_; + cv::Mat frequency_image(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + + for (const auto & weak_first_single_ring : weak_first_pcl_input_ring_array) { + if (weak_first_single_ring.points.size() < 2) { + continue; + } + std::vector deleted_azimuths; + std::vector deleted_distances; + pcl::PointCloud temp_segment; + + bool keep_next = false; + uint ring_id = weak_first_single_ring.points.front().ring; + for (auto iter = std::begin(weak_first_single_ring) + 1; + iter != std::end(weak_first_single_ring) - 1; ++iter) { + const float min_dist = std::min(iter->distance, (iter + 1)->distance); + const float max_dist = std::max(iter->distance, (iter + 1)->distance); + float azimuth_diff = (iter + 1)->azimuth - iter->azimuth; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + + if (max_dist < min_dist * weak_first_distance_ratio_ && azimuth_diff < max_azimuth_diff) { + temp_segment.points.push_back(*iter); + keep_next = true; + } else if (keep_next) { + temp_segment.points.push_back(*iter); + keep_next = false; + // Analyse segment points here + } else { + // Log the deleted azimuth and its distance for analysis + deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); + deleted_distances.push_back(iter->distance); + noise_output->points.push_back(*iter); + } + } + // Analyse last segment points here + std::vector noise_frequency(horizontal_bins, 0); + uint current_deleted_index = 0; + uint current_temp_segment_index = 0; + for (uint i = 0; i < noise_frequency.size() - 1; i++) { + if (deleted_azimuths.size() == 0) { + continue; + } + while ((uint)deleted_azimuths[current_deleted_index] < + ((i + 1) * (36000 / horizontal_bins)) && + current_deleted_index < (deleted_azimuths.size() - 1)) { + noise_frequency[i] = noise_frequency[i] + 1; + current_deleted_index++; + } + if (temp_segment.points.size() == 0) { + continue; + } + while ((temp_segment.points[current_temp_segment_index].azimuth < 0.f + ? 0.f + : temp_segment.points[current_temp_segment_index].azimuth) < + ((i + 1) * (36000 / horizontal_bins)) && + current_temp_segment_index < (temp_segment.points.size() - 1)) { + if (noise_frequency[i] < weak_first_local_noise_threshold_) { + pcl_output->points.push_back(temp_segment.points[current_temp_segment_index]); + } else { + noise_frequency[i] = noise_frequency[i] + 1; + noise_output->points.push_back(temp_segment.points[current_temp_segment_index]); + } + current_temp_segment_index++; + frequency_image.at(ring_id, i) = noise_frequency[i]; + } + } + } + + // Ring outlier filter for normal points + for (const auto & input_ring : pcl_input_ring_array) { + if (input_ring.size() < 2) { + continue; + } + pcl::PointCloud temp_segment; + bool keep_next = false; + // uint ring_id = input_ring.points.front().ring; + for (auto iter = std::begin(input_ring) + 1; iter != std::end(input_ring) - 1; ++iter) { + const float min_dist = std::min(iter->distance, (iter + 1)->distance); + const float max_dist = std::max(iter->distance, (iter + 1)->distance); + float azimuth_diff = (iter + 1)->azimuth - iter->azimuth; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + + if (max_dist < min_dist * general_distance_ratio_ && azimuth_diff < max_azimuth_diff) { + temp_segment.points.push_back(*iter); + keep_next = true; + } else if (keep_next) { + temp_segment.points.push_back(*iter); + keep_next = false; + // Analyse segment points here + } else { + // Log the deleted azimuth and its distance for analysis + // deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); + // deleted_distances.push_back(iter->distance); + noise_output->points.push_back(*iter); + } + } + for (const auto & tmp_p : temp_segment.points) { + pcl_output->points.push_back(tmp_p); + } + } + // Threshold for diagnostics (tunable) + cv::Mat binary_image; + cv::inRange(frequency_image, weak_first_local_noise_threshold_, 255, binary_image); + int num_pixels = cv::countNonZero(binary_image); + float filled = + static_cast(num_pixels) / static_cast(vertical_bins * horizontal_bins); + visibility_ = 1.0f - filled; + // Visualization of histogram + cv::Mat frequency_image_colorized; + // Multiply bins by four to get pretty colours + cv::applyColorMap(frequency_image * 4, frequency_image_colorized, cv::COLORMAP_JET); + sensor_msgs::msg::Image::SharedPtr frequency_image_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frequency_image_colorized).toImageMsg(); + + // Publish histogram image + image_pub_.publish(frequency_image_msg); + autoware_debug_msgs::msg::Float32Stamped visibility_msg; + visibility_msg.data = (1.0f - filled); + visibility_msg.stamp = now(); + visibility_pub_->publish(visibility_msg); + + // Publish noise points + sensor_msgs::msg::PointCloud2 noise_output_msg; + pcl::toROSMsg(*noise_output, noise_output_msg); + noise_output_msg.header = input->header; + noise_cloud_pub_->publish(noise_output_msg); + + // Publish filtered pointcloud + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; +} + +rcl_interfaces::msg::SetParametersResult DualReturnOutlierFilterComponent::paramCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (get_param(p, "weak_first_distance_ratio", weak_first_distance_ratio_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new weak first distance ratio to: %f.", weak_first_distance_ratio_); + } + if (get_param(p, "general_distance_ratio", general_distance_ratio_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new general distance ratio to: %f.", general_distance_ratio_); + } + if (get_param(p, "weak_first_local_noise_threshold", weak_first_local_noise_threshold_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new weak first local noise threshold to: %d.", + weak_first_local_noise_threshold_); + } + if (get_param(p, "vertical_bins", vertical_bins_)) { + RCLCPP_DEBUG(get_logger(), "Setting new vertical_bins to: %d.", vertical_bins_); + } + if (get_param(p, "max_azimuth_diff", max_azimuth_diff_)) { + RCLCPP_DEBUG(get_logger(), "Setting new max_azimuth_diff to: %f.", max_azimuth_diff_); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DualReturnOutlierFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.cpp new file mode 100644 index 0000000000000..dc00e0e86ac5a --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -0,0 +1,325 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pointcloud_preprocessor/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace +{ +bool transformPointcloud( + const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2, + const std::string & target_frame, sensor_msgs::msg::PointCloud2 & output) +{ + rclcpp::Clock clock{RCL_ROS_TIME}; + geometry_msgs::msg::TransformStamped tf_stamped{}; + try { + tf_stamped = tf2.lookupTransform( + target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("pointcloud_processor").get_child("occupancy_grid_map_outlier_filter"), + clock, 5000, "%s", ex.what()); + return false; + } + // transform pointcloud + Eigen::Matrix4f tf_matrix = tf2::transformToEigen(tf_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(tf_matrix, input, output); + output.header.stamp = input.header.stamp; + output.header.frame_id = target_frame; + return true; +} + +geometry_msgs::msg::PoseStamped getPoseStamped( + const tf2_ros::Buffer & tf2, const std::string & target_frame_id, + const std::string & src_frame_id, const rclcpp::Time & time) +{ + rclcpp::Clock clock{RCL_ROS_TIME}; + geometry_msgs::msg::TransformStamped tf_stamped{}; + try { + tf_stamped = + tf2.lookupTransform(target_frame_id, src_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("pointcloud_processor").get_child("occupancy_grid_map_outlier_filter"), + clock, 5000, "%s", ex.what()); + } + return autoware_utils::transform2pose(tf_stamped); +} + +boost::optional getCost( + const nav_msgs::msg::OccupancyGrid & map, const double & x, const double & y) +{ + const auto & map_position = map.info.origin.position; + const auto & map_resolution = map.info.resolution; + const double map_height_m = map.info.height /* cell size */ * map_resolution; + const double map_width_m = map.info.width /* cell size */ * map_resolution; + const double map_min_x = map_position.x; + const double map_max_x = map_position.x + map_width_m; + const double map_min_y = map_position.y; + const double map_max_y = map_position.y + map_height_m; + + if (map_min_x < x && x < map_max_x && map_min_y < y && y < map_max_y) { + unsigned int map_cell_x{}; + unsigned int map_cell_y{}; + map_cell_x = std::floor((x - map_position.x) / map_resolution); + map_cell_y = std::floor((y - map_position.y) / map_resolution); + size_t index = map_cell_y * map.info.width + map_cell_x; + return map.data.at(index); + } + return boost::none; +} + +} // namespace + +namespace pointcloud_preprocessor +{ +RadiusSearch2dfilter::RadiusSearch2dfilter(rclcpp::Node & node) +{ + search_radius_ = node.declare_parameter("radius_search_2d_filter.search_radius", 1.0f); + min_points_and_distance_ratio_ = + node.declare_parameter("radius_search_2d_filter.min_points_and_distance_ratio", 400.0f); + min_points_ = node.declare_parameter("radius_search_2d_filter.min_points", 4); + max_points_ = node.declare_parameter("radius_search_2d_filter.max_points", 70); + kd_tree_ = boost::make_shared>(false); +} + +void RadiusSearch2dfilter::filter( + const PclPointCloud & input, const Pose & pose, PclPointCloud & output, PclPointCloud & outlier) +{ + const auto & xyz_cloud = input; + pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); + xy_cloud->points.resize(xyz_cloud.points.size()); + for (size_t i = 0; i < xyz_cloud.points.size(); ++i) { + xy_cloud->points[i].x = xyz_cloud.points[i].x; + xy_cloud->points[i].y = xyz_cloud.points[i].y; + } + + std::vector k_indices(xy_cloud->points.size()); + std::vector k_dists(xy_cloud->points.size()); + kd_tree_->setInputCloud(xy_cloud); + for (size_t i = 0; i < xy_cloud->points.size(); ++i) { + const float distance = + std::hypot(xy_cloud->points[i].x - pose.position.x, xy_cloud->points[i].y - pose.position.y); + const int min_points_threshold = std::min( + std::max(static_cast(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), + max_points_); + const int points_num = kd_tree_->radiusSearch(i, search_radius_, k_indices, k_dists); + + if (min_points_threshold <= points_num) { + output.points.push_back(xyz_cloud.points.at(i)); + } else { + outlier.points.push_back(xyz_cloud.points.at(i)); + } + } +} + +void RadiusSearch2dfilter::filter( + const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose, + PclPointCloud & output, PclPointCloud & outlier) +{ + const auto & high_conf_xyz_cloud = high_conf_input; + const auto & low_conf_xyz_cloud = low_conf_input; + pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); + xy_cloud->points.resize(low_conf_xyz_cloud.points.size() + high_conf_xyz_cloud.points.size()); + for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) { + xy_cloud->points[i].x = low_conf_xyz_cloud.points[i].x; + xy_cloud->points[i].y = low_conf_xyz_cloud.points[i].y; + } + for (size_t i = low_conf_xyz_cloud.points.size(); i < xy_cloud->points.size(); ++i) { + xy_cloud->points[i].x = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].x; + xy_cloud->points[i].y = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].y; + } + + std::vector k_indices(xy_cloud->points.size()); + std::vector k_dists(xy_cloud->points.size()); + kd_tree_->setInputCloud(xy_cloud); + for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) { + const float distance = + std::hypot(xy_cloud->points[i].x - pose.position.x, xy_cloud->points[i].y - pose.position.y); + const int min_points_threshold = std::min( + std::max(static_cast(min_points_and_distance_ratio_ / distance + 0.5f), min_points_), + max_points_); + const int points_num = kd_tree_->radiusSearch(i, search_radius_, k_indices, k_dists); + + if (min_points_threshold <= points_num) { + output.points.push_back(low_conf_xyz_cloud.points.at(i)); + } else { + outlier.points.push_back(low_conf_xyz_cloud.points.at(i)); + } + } +} + +OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( + const rclcpp::NodeOptions & options) +: Node("OccupancyGridMapOutlierFilter", options) +{ + /* params */ + map_frame_ = declare_parameter("map_frame", "map"); + base_link_frame_ = declare_parameter("base_link_frame", "base_link"); + cost_threshold_ = declare_parameter("cost_threshold", 45); + auto use_radius_search_2d_filter = declare_parameter("use_radius_search_2d_filter", true); + auto enable_debugger = declare_parameter("enable_debugger", false); + + /* tf */ + tf2_ = std::make_shared(get_clock()); + tf2_listener_ = std::make_shared(*tf2_); + + /* Subscriber and publisher */ + pointcloud_sub_.subscribe(this, "~/input/pointcloud", rmw_qos_profile_sensor_data); + occupancy_grid_map_sub_.subscribe( + this, "~/input/occupancy_grid_map", rclcpp::QoS{1}.get_rmw_qos_profile()); + sync_ptr_ = std::make_shared(SyncPolicy(5), occupancy_grid_map_sub_, pointcloud_sub_); + sync_ptr_->registerCallback(std::bind( + &OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2, this, + std::placeholders::_1, std::placeholders::_2)); + pointcloud_pub_ = create_publisher("~/output/pointcloud", rclcpp::SensorDataQoS()); + + /* Radius search 2d filter */ + if (use_radius_search_2d_filter) { + radius_search_2d_filter_ptr_ = std::make_shared(*this); + } + /* debugger */ + if (enable_debugger) { + debugger_ptr_ = std::make_shared(*this); + } +} + +void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( + const OccupancyGrid::ConstSharedPtr & input_ogm, const PointCloud2::ConstSharedPtr & input_pc) +{ + // Transform to occupancy grid map frame + PointCloud2 ogm_frame_pc{}; + if (!transformPointcloud(*input_pc, *tf2_, input_ogm->header.frame_id, ogm_frame_pc)) { + return; + } + // Occupancy grid map based filter + PclPointCloud high_confidence_pc{}; + PclPointCloud low_confidence_pc{}; + filterByOccupancyGridMap(*input_ogm, ogm_frame_pc, high_confidence_pc, low_confidence_pc); + // Apply Radius search 2d filter for low confidence pointcloud + PclPointCloud filtered_low_confidence_pc{}; + PclPointCloud outlier_pc{}; + if (radius_search_2d_filter_ptr_) { + auto pc_frame_pose_stamped = getPoseStamped( + *tf2_, input_ogm->header.frame_id, input_pc->header.frame_id, input_ogm->header.stamp); + radius_search_2d_filter_ptr_->filter( + high_confidence_pc, low_confidence_pc, pc_frame_pose_stamped.pose, filtered_low_confidence_pc, + outlier_pc); + } else { + outlier_pc = low_confidence_pc; + } + // Concatenate high confidence pointcloud from occupancy grid map and non-outlier pointcloud + PclPointCloud concat_pc = high_confidence_pc + filtered_low_confidence_pc; + // Convert to ros msg + { + PointCloud2 ogm_frame_filtered_pc{}; + auto base_link_frame_filtered_pc_ptr = std::make_unique(); + pcl::toROSMsg(concat_pc, ogm_frame_filtered_pc); + ogm_frame_filtered_pc.header = ogm_frame_pc.header; + if (!transformPointcloud( + ogm_frame_filtered_pc, *tf2_, base_link_frame_, *base_link_frame_filtered_pc_ptr)) { + return; + } + auto pc_ptr = std::make_unique(); + pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); + } + if (debugger_ptr_) { + debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header); + debugger_ptr_->publishLowConfidence(filtered_low_confidence_pc, ogm_frame_pc.header); + debugger_ptr_->publishOutlier(outlier_pc, ogm_frame_pc.header); + } +} + +void OccupancyGridMapOutlierFilterComponent::filterByOccupancyGridMap( + const OccupancyGrid & occupancy_grid_map, const PointCloud2 & pointcloud, + PclPointCloud & high_confidence, PclPointCloud & low_confidence) +{ + for (sensor_msgs::PointCloud2ConstIterator x(pointcloud, "x"), y(pointcloud, "y"), + z(pointcloud, "z"); + x != x.end(); ++x, ++y, ++z) { + const auto cost = getCost(occupancy_grid_map, *x, *y); + if (cost) { + if (cost_threshold_ < *cost) { + high_confidence.push_back(pcl::PointXYZ(*x, *y, *z)); + } else { + low_confidence.push_back(pcl::PointXYZ(*x, *y, *z)); + } + } else { + high_confidence.push_back(pcl::PointXYZ(*x, *y, *z)); + } + } +} + +OccupancyGridMapOutlierFilterComponent::Debugger::Debugger( + OccupancyGridMapOutlierFilterComponent & node) +: node_(node) +{ + outlier_pointcloud_pub_ = node.create_publisher( + "~/output/debug/outlier/pointcloud", rclcpp::SensorDataQoS()); + low_confidence_pointcloud_pub_ = node.create_publisher( + "~/output/debug/low_confidence/pointcloud", rclcpp::SensorDataQoS()); + high_confidence_pointcloud_pub_ = node.create_publisher( + "~/output/debug/high_confidence/pointcloud", rclcpp::SensorDataQoS()); +} + +void OccupancyGridMapOutlierFilterComponent::Debugger::publishOutlier( + const PclPointCloud & input, const Header & header) +{ + auto output_ptr = std::make_unique(); + transformToBaseLink(input, header, *output_ptr); + outlier_pointcloud_pub_->publish(std::move(output_ptr)); +} +void OccupancyGridMapOutlierFilterComponent::Debugger::publishHighConfidence( + const PclPointCloud & input, const Header & header) +{ + auto output_ptr = std::make_unique(); + transformToBaseLink(input, header, *output_ptr); + high_confidence_pointcloud_pub_->publish(std::move(output_ptr)); +} + +void OccupancyGridMapOutlierFilterComponent::Debugger::publishLowConfidence( + const PclPointCloud & input, const Header & header) +{ + auto output_ptr = std::make_unique(); + transformToBaseLink(input, header, *output_ptr); + low_confidence_pointcloud_pub_->publish(std::move(output_ptr)); +} + +void OccupancyGridMapOutlierFilterComponent::Debugger::transformToBaseLink( + const PclPointCloud & pcl_input, const Header & header, PointCloud2 & output) +{ + PointCloud2 ros_input{}; + pcl::toROSMsg(pcl_input, ros_input); + ros_input.header = header; + transformPointcloud(ros_input, *(node_.tf2_), node_.base_link_frame_, output); +} + +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::OccupancyGridMapOutlierFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp new file mode 100644 index 0000000000000..3e31e16309b7c --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp @@ -0,0 +1,92 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp" + +#include +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +RadiusSearch2DOutlierFilterComponent::RadiusSearch2DOutlierFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("RadiusSearch2DOutlierFilter", options) +{ + // set initial parameters + { + min_neighbors_ = static_cast(declare_parameter("min_neighbors", 5)); + search_radius_ = static_cast(declare_parameter("search_radius", 0.2)); + } + + kd_tree_ = boost::make_shared>(false); + + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RadiusSearch2DOutlierFilterComponent::paramCallback, this, _1)); +} + +void RadiusSearch2DOutlierFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + pcl::PointCloud::Ptr xyz_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*input, *xyz_cloud); + + pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); + xy_cloud->points.resize(xyz_cloud->points.size()); + for (size_t i = 0; i < xyz_cloud->points.size(); ++i) { + xy_cloud->points[i].x = xyz_cloud->points[i].x; + xy_cloud->points[i].y = xyz_cloud->points[i].y; + } + + std::vector k_indices(xy_cloud->points.size()); + std::vector k_sqr_distances(xy_cloud->points.size()); + kd_tree_->setInputCloud(xy_cloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + for (size_t i = 0; i < xy_cloud->points.size(); ++i) { + size_t k = kd_tree_->radiusSearch(i, search_radius_, k_indices, k_sqr_distances); + if (k >= min_neighbors_) { + pcl_output->points.push_back(xyz_cloud->points.at(i)); + } + } + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; +} + +rcl_interfaces::msg::SetParametersResult RadiusSearch2DOutlierFilterComponent::paramCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (get_param(p, "min_neighbors", min_neighbors_)) { + RCLCPP_DEBUG(get_logger(), "Setting new min neighbors to: %zu.", min_neighbors_); + } + if (get_param(p, "search_radius", search_radius_)) { + RCLCPP_DEBUG(get_logger(), "Setting new search radius to: %f.", search_radius_); + } + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} + +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::RadiusSearch2DOutlierFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp new file mode 100644 index 0000000000000..580c0c33422b1 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -0,0 +1,150 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" + +#include +#include +#include + +#include +#include + +namespace pointcloud_preprocessor +{ +RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) +: Filter("RingOutlierFilter", options) +{ + // set initial parameters + { + distance_ratio_ = static_cast(declare_parameter("distance_ratio", 1.03)); + object_length_threshold_ = + static_cast(declare_parameter("object_length_threshold", 0.1)); + num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); + } + + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RingOutlierFilterComponent::paramCallback, this, _1)); +} + +void RingOutlierFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + pcl::PointCloud::Ptr pcl_input( + new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + + if (pcl_input->points.empty()) { + return; + } + std::vector> pcl_input_ring_array; + pcl_input_ring_array.resize(128); // TODO(Yamato Ando) + for (const auto & p : pcl_input->points) { + pcl_input_ring_array.at(p.ring).push_back(p); + } + + pcl::PointCloud::Ptr pcl_output( + new pcl::PointCloud); + pcl_output->points.reserve(pcl_input->points.size()); + + pcl::PointCloud pcl_tmp; + custom_pcl::PointXYZI p{}; + for (const auto & ring_pointcloud : pcl_input_ring_array) { + if (ring_pointcloud.points.size() < 2) { + continue; + } + + for (auto iter = std::begin(ring_pointcloud.points); + iter != std::end(ring_pointcloud.points) - 1; ++iter) { + p.x = iter->x; + p.y = iter->y; + p.z = iter->z; + p.intensity = iter->intensity; + pcl_tmp.points.push_back(p); + // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) { + const float min_dist = std::min(iter->distance, (iter + 1)->distance); + const float max_dist = std::max(iter->distance, (iter + 1)->distance); + float azimuth_diff = (iter + 1)->azimuth - iter->azimuth; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + + if (max_dist < min_dist * distance_ratio_ && azimuth_diff < 100.f) { + continue; + } + // same code + if ( + static_cast(pcl_tmp.points.size()) > num_points_threshold_ || + (pcl_tmp.points.front().x - pcl_tmp.points.back().x) * + (pcl_tmp.points.front().x - pcl_tmp.points.back().x) + + (pcl_tmp.points.front().y - pcl_tmp.points.back().y) * + (pcl_tmp.points.front().y - pcl_tmp.points.back().y) + + (pcl_tmp.points.front().z - pcl_tmp.points.back().z) * + (pcl_tmp.points.front().z - pcl_tmp.points.back().z) >= + object_length_threshold_ * object_length_threshold_) { + for (const auto & tmp_p : pcl_tmp.points) { + pcl_output->points.push_back(tmp_p); + } + } + pcl_tmp.points.clear(); + } + + // same code + if ( + static_cast(pcl_tmp.points.size()) > num_points_threshold_ || + (pcl_tmp.points.front().x - pcl_tmp.points.back().x) * + (pcl_tmp.points.front().x - pcl_tmp.points.back().x) + + (pcl_tmp.points.front().y - pcl_tmp.points.back().y) * + (pcl_tmp.points.front().y - pcl_tmp.points.back().y) + + (pcl_tmp.points.front().z - pcl_tmp.points.back().z) * + (pcl_tmp.points.front().z - pcl_tmp.points.back().z) >= + object_length_threshold_ * object_length_threshold_) { + for (const auto & tmp_p : pcl_tmp.points) { + pcl_output->points.push_back(tmp_p); + } + } + pcl_tmp.points.clear(); + } + + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; +} + +rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (get_param(p, "distance_ratio", distance_ratio_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance ratio to: %f.", distance_ratio_); + } + if (get_param(p, "object_length_threshold", object_length_threshold_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new object length threshold to: %f.", object_length_threshold_); + } + if (get_param(p, "num_points_threshold", num_points_threshold_)) { + RCLCPP_DEBUG(get_logger(), "Setting new num_points_threshold to: %d.", num_points_threshold_); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::RingOutlierFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp new file mode 100644 index 0000000000000..88b8c9d7a36a3 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp @@ -0,0 +1,97 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp" + +#include +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +VoxelGridOutlierFilterComponent::VoxelGridOutlierFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("VoxelGridOutlierFilter", options) +{ + // set initial parameters + { + voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); + voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); + voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); + voxel_points_threshold_ = static_cast(declare_parameter("voxel_points_threshold", 2)); + } + + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&VoxelGridOutlierFilterComponent::paramCallback, this, _1)); +} +void VoxelGridOutlierFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_voxelized_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + pcl_voxelized_input->points.reserve(pcl_input->points.size()); + voxel_filter.setInputCloud(pcl_input); + voxel_filter.setSaveLeafLayout(true); + voxel_filter.setLeafSize(voxel_size_x_, voxel_size_y_, voxel_size_z_); + voxel_filter.setMinimumPointsNumberPerVoxel(voxel_points_threshold_); + voxel_filter.filter(*pcl_voxelized_input); + + pcl_output->points.reserve(pcl_input->points.size()); + for (size_t i = 0; i < pcl_input->points.size(); ++i) { + const int index = voxel_filter.getCentroidIndexAt(voxel_filter.getGridCoordinates( + pcl_input->points.at(i).x, pcl_input->points.at(i).y, pcl_input->points.at(i).z)); + if (index != -1) { // not empty voxel + pcl_output->points.push_back(pcl_input->points.at(i)); + } + } + + pcl::toROSMsg(*pcl_output, output); + output.header = input->header; +} + +rcl_interfaces::msg::SetParametersResult VoxelGridOutlierFilterComponent::paramCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (get_param(p, "voxel_size_x", voxel_size_x_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_x_); + } + if (get_param(p, "voxel_size_y", voxel_size_y_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_y_); + } + if (get_param(p, "voxel_size_z", voxel_size_z_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_z_); + } + if (get_param(p, "voxel_points_threshold", voxel_points_threshold_)) { + RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %d.", voxel_points_threshold_); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::VoxelGridOutlierFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp new file mode 100644 index 0000000000000..8cadce439f6a9 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp @@ -0,0 +1,93 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: passthrough.cpp 36194 2011-02-23 07:49:21Z rusu $ + * + */ + +#include "pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp" + +#include +#include +#include + +#include + +namespace pointcloud_preprocessor +{ +PassThroughFilterComponent::PassThroughFilterComponent(const rclcpp::NodeOptions & options) +: Filter("PassThroughFilter", options) +{ + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&PassThroughFilterComponent::paramCallback, this, _1)); +} + +void PassThroughFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + output = *input; +} + +rcl_interfaces::msg::SetParametersResult PassThroughFilterComponent::paramCallback( + [[maybe_unused]] const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + // write me + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PassThroughFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp b/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp new file mode 100644 index 0000000000000..0b2602220b28d --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp @@ -0,0 +1,127 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp" + +#include +#include +#include + +#include +#include + +namespace pointcloud_preprocessor +{ +PassThroughFilterUInt16Component::PassThroughFilterUInt16Component( + const rclcpp::NodeOptions & options) +: Filter("PassThroughFilterUInt16", options) +{ + // set initial parameters + { + int filter_min = static_cast(declare_parameter("filter_limit_min", 0)); + int filter_max = static_cast(declare_parameter("filter_limit_max", 127)); + impl_.setFilterLimits(filter_min, filter_max); + + impl_.setFilterFieldName( + static_cast(declare_parameter("filter_field_name", "ring"))); + impl_.setKeepOrganized(static_cast(declare_parameter("keep_organized", false))); + impl_.setFilterLimitsNegative( + static_cast(declare_parameter("filter_limit_negative", false))); + } + + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&PassThroughFilterUInt16Component::paramCallback, this, _1)); +} + +void PassThroughFilterUInt16Component::filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); +} + +rcl_interfaces::msg::SetParametersResult PassThroughFilterUInt16Component::paramCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + std::uint16_t filter_min, filter_max; + impl_.getFilterLimits(filter_min, filter_max); + + // Check the current values for filter min-max + if (get_param(p, "filter_limit_min", filter_min)) { + RCLCPP_DEBUG( + get_logger(), "Setting the minimum filtering value a point will be considered from to: %d.", + filter_min); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + // Check the current values for filter min-max + if (get_param(p, "filter_limit_max", filter_max)) { + RCLCPP_DEBUG( + get_logger(), "Setting the maximum filtering value a point will be considered from to: %d.", + filter_max); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + + // Check the current value for the filter field + std::string filter_field_name; + get_param(p, "filter_field_name", filter_field_name); + if (impl_.getFilterFieldName() != filter_field_name) { + // Set the filter field if different + impl_.setFilterFieldName(filter_field_name); + RCLCPP_DEBUG(get_logger(), "Setting the filter field name to: %s.", filter_field_name.c_str()); + } + + // Check the current value for keep_organized + bool keep_organized; + get_param(p, "keep_organized", keep_organized); + if (impl_.getKeepOrganized() != keep_organized) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter keep_organized value to: %s.", + keep_organized ? "true" : "false"); + // Call the virtual method in the child + impl_.setKeepOrganized(keep_organized); + } + + // Check the current value for the negative flag + bool filter_limit_negative; + get_param(p, "filter_limit_negative", filter_limit_negative); + if (impl_.getFilterLimitsNegative() != filter_limit_negative) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter negative flag to: %s.", + filter_limit_negative ? "true" : "false"); + // Call the virtual method in the child + impl_.setFilterLimitsNegative(filter_limit_negative); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PassThroughFilterUInt16Component) diff --git a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp b/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp new file mode 100644 index 0000000000000..cd589e252544b --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp @@ -0,0 +1,285 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include "pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp" + +////////////////////////////////////////////////////////////////////////// +void pcl::PassThroughUInt16::applyFilter(PCLPointCloud2 & output) +{ + if (!input_) { + PCL_ERROR("[pcl::%s::applyFilter] Input dataset not given!\n", getClassName().c_str()); + output.width = output.height = 0; + output.data.clear(); + return; + } + + // If fields x/y/z are not present, we cannot filter + if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1) { + PCL_ERROR( + "[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", + getClassName().c_str()); + output.width = output.height = 0; + output.data.clear(); + return; + } + + int nr_points = input_->width * input_->height; + + // Check if we're going to keep the organized structure of the cloud or not + if (keep_organized_) { + if (filter_field_name_.empty()) { + // Silly - if no filtering is actually done, and we want to keep the data organized, + // just copy everything. Any optimizations that can be done here??? + output = *input_; + return; + } + + output.width = input_->width; + output.height = input_->height; + // Check what the user value is: if !finite, set is_dense to false, true otherwise + if (!std::isfinite(user_filter_value_)) { + output.is_dense = false; + } else { + output.is_dense = input_->is_dense; + } + } else { + // Copy the header (and thus the frame_id) + allocate enough space for points + output.height = 1; // filtering breaks the organized structure + // Because we're doing explicit checks for isfinite, is_dense = true + output.is_dense = true; + } + output.row_step = input_->row_step; + output.point_step = input_->point_step; + output.is_bigendian = input_->is_bigendian; + output.data.resize(input_->data.size()); + + removed_indices_->resize(input_->data.size()); + + int nr_p = 0; + int nr_removed_p = 0; + // Create the first xyz_offset + Eigen::Array4i xyz_offset( + input_->fields[x_idx_].offset, input_->fields[y_idx_].offset, input_->fields[z_idx_].offset, 0); + + Eigen::Vector4f pt = Eigen::Vector4f::Zero(); + // If we don't want to process the entire cloud, + // but rather filter points far away from the viewpoint first... + if (!filter_field_name_.empty()) { + // Get the distance field index + int distance_idx = pcl::getFieldIndex(*input_, filter_field_name_); + if (distance_idx == -1) { + PCL_WARN( + "[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName().c_str(), + distance_idx); + output.width = output.height = 0; + output.data.clear(); + return; + } + + // @todo fixme + if (input_->fields[distance_idx].datatype != pcl::PCLPointField::UINT16) { + PCL_ERROR( + "[pcl::%s::downsample] Distance filtering requested, but distances are not uint16 in the " + "dataset! Only UINT16 distances are supported right now.\n", + getClassName().c_str()); + output.width = output.height = 0; + output.data.clear(); + return; + } + + std::uint16_t badpt = user_filter_value_; + // Check whether we need to store filtered valued in place + if (keep_organized_) { + std::uint16_t distance_value = 0; + // Go over all points + for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step) { + // Copy all the fields + memcpy( + &output.data[cp * output.point_step], &input_->data[cp * output.point_step], + output.point_step); + + // Get the distance value + memcpy( + &distance_value, + &input_->data[cp * input_->point_step + input_->fields[distance_idx].offset], + sizeof(std::uint16_t)); + + if (filter_limit_negative_) { + // Use a threshold for cutting out points which inside the interval + if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) { + // Unoptimized memcpys: assume fields x, y, z are in random order + memcpy(&output.data[xyz_offset[0]], &badpt, sizeof(std::uint16_t)); + memcpy(&output.data[xyz_offset[1]], &badpt, sizeof(std::uint16_t)); + memcpy(&output.data[xyz_offset[2]], &badpt, sizeof(std::uint16_t)); + continue; + } else { + if (extract_removed_indices_) { + (*removed_indices_)[nr_removed_p++] = cp; + } + } + } else { + // Use a threshold for cutting out points which are too close/far away + if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) { + // Unoptimized memcpys: assume fields x, y, z are in random order + memcpy(&output.data[xyz_offset[0]], &badpt, sizeof(std::uint16_t)); + memcpy(&output.data[xyz_offset[1]], &badpt, sizeof(std::uint16_t)); + memcpy(&output.data[xyz_offset[2]], &badpt, sizeof(std::uint16_t)); + continue; + } else { + if (extract_removed_indices_) { + (*removed_indices_)[nr_removed_p++] = cp; + } + } + } + } + } else { // Remove filtered points + // Go over all points + std::uint16_t distance_value = 0; + for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step) { + // Get the distance value + memcpy( + &distance_value, + &input_->data[cp * input_->point_step + input_->fields[distance_idx].offset], + sizeof(std::uint16_t)); + + // Remove NAN/INF/-INF values. We expect passthrough to output clean valid data. + if (!std::isfinite(distance_value)) { + if (extract_removed_indices_) { + (*removed_indices_)[nr_removed_p++] = cp; + } + continue; + } + + if (filter_limit_negative_) { + // Use a threshold for cutting out points which inside the interval + if (distance_value < filter_limit_max_ && distance_value > filter_limit_min_) { + if (extract_removed_indices_) { + (*removed_indices_)[nr_removed_p] = cp; + nr_removed_p++; + } + continue; + } + } else { + // Use a threshold for cutting out points which are too close/far away + if (distance_value > filter_limit_max_ || distance_value < filter_limit_min_) { + if (extract_removed_indices_) { + (*removed_indices_)[nr_removed_p] = cp; + nr_removed_p++; + } + continue; + } + } + + // Unoptimized memcpys: assume fields x, y, z are in random order + memcpy(&pt[0], &input_->data[xyz_offset[0]], sizeof(std::uint16_t)); + memcpy(&pt[1], &input_->data[xyz_offset[1]], sizeof(std::uint16_t)); + memcpy(&pt[2], &input_->data[xyz_offset[2]], sizeof(std::uint16_t)); + + // Check if the point is invalid + if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { + if (extract_removed_indices_) { + (*removed_indices_)[nr_removed_p] = cp; + nr_removed_p++; + } + continue; + } + + // Copy all the fields + memcpy( + &output.data[nr_p * output.point_step], &input_->data[cp * output.point_step], + output.point_step); + nr_p++; + } + output.width = nr_p; + } // !keep_organized_ + } else { // No distance filtering, process all data. + // No need to check for is_organized here as we did it above + for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step) { + // Unoptimized memcpys: assume fields x, y, z are in random order + memcpy(&pt[0], &input_->data[xyz_offset[0]], sizeof(std::uint16_t)); + memcpy(&pt[1], &input_->data[xyz_offset[1]], sizeof(std::uint16_t)); + memcpy(&pt[2], &input_->data[xyz_offset[2]], sizeof(std::uint16_t)); + + // Check if the point is invalid + if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { + if (extract_removed_indices_) { + (*removed_indices_)[nr_removed_p] = cp; + nr_removed_p++; + } + continue; + } + + // Copy all the fields + memcpy( + &output.data[nr_p * output.point_step], &input_->data[cp * output.point_step], + output.point_step); + nr_p++; + } + output.width = nr_p; + } + + output.row_step = output.point_step * output.width; + output.data.resize(output.width * output.height * output.point_step); + + removed_indices_->resize(nr_removed_p); +} + +#ifndef PCL_NO_PRECOMPILE +#include + +#include + +// Instantiations of specific point types +PCL_INSTANTIATE(PassThroughUInt16, PCL_XYZ_POINT_TYPES) + +#endif // PCL_NO_PRECOMPILE diff --git a/sensing/pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp b/sensing/pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp new file mode 100644 index 0000000000000..980d77a170997 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp @@ -0,0 +1,79 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp" + +#include + +namespace pointcloud_preprocessor +{ +PointcloudAccumulatorComponent::PointcloudAccumulatorComponent(const rclcpp::NodeOptions & options) +: Filter("PointcloudAccumulator", options) +{ + // set initial parameters + { + accumulation_time_sec_ = static_cast(declare_parameter("accumulation_time_sec", 2.0)); + pointcloud_buffer_.set_capacity( + static_cast(declare_parameter("pointcloud_buffer_size", 50))); + } + + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&PointcloudAccumulatorComponent::paramCallback, this, _1)); +} + +void PointcloudAccumulatorComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + boost::mutex::scoped_lock lock(mutex_); + pointcloud_buffer_.push_front(input); + rclcpp::Time last_time = input->header.stamp; + pcl::PointCloud pcl_input; + pcl::PointCloud pcl_output; + for (size_t i = 0; i < pointcloud_buffer_.size(); i++) { + if (accumulation_time_sec_ < (last_time - pointcloud_buffer_.at(i)->header.stamp).seconds()) { + break; + } + pcl::fromROSMsg(*pointcloud_buffer_.at(i), pcl_input); + pcl_output += pcl_input; + } + pcl::toROSMsg(pcl_output, output); + output.header = input->header; +} + +rcl_interfaces::msg::SetParametersResult PointcloudAccumulatorComponent::paramCallback( + const std::vector & p) +{ + boost::mutex::scoped_lock lock(mutex_); + + if (get_param(p, "accumulation_time_sec", accumulation_time_sec_)) { + RCLCPP_DEBUG(get_logger(), "Setting new accumulation time to: %f.", accumulation_time_sec_); + } + int pointcloud_buffer_size; + if (get_param(p, "pointcloud_buffer_size", pointcloud_buffer_size)) { + pointcloud_buffer_.set_capacity((size_t)pointcloud_buffer_size); + RCLCPP_DEBUG(get_logger(), "Setting new buffer size to: %d.", pointcloud_buffer_size); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PointcloudAccumulatorComponent) diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp new file mode 100644 index 0000000000000..502c85d1746e8 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp @@ -0,0 +1,273 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp" + +#include "pointcloud_preprocessor/filter.hpp" + +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace pointcloud_preprocessor +{ +Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions & node_options) +: Node("LaneletMapFilter", node_options) +{ + using std::placeholders::_1; + + // Set parameters + { + voxel_size_x_ = declare_parameter("voxel_size_x", 0.04); + voxel_size_y_ = declare_parameter("voxel_size_y", 0.04); + } + + // Set publisher + { + filtered_pointcloud_pub_ = + this->create_publisher("output", rclcpp::SensorDataQoS()); + } + + // Set subscriber + { + map_sub_ = this->create_subscription( + "input/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&Lanelet2MapFilterComponent::mapCallback, this, _1)); + pointcloud_sub_ = this->create_subscription( + "input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&Lanelet2MapFilterComponent::pointcloudCallback, this, _1)); + } + + // Set parameter reconfigure + { + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&Lanelet2MapFilterComponent::paramCallback, this, _1)); + } + + // Set tf + { + rclcpp::Clock::SharedPtr ros_clock = std::make_shared(RCL_ROS_TIME); + tf_buffer_ = std::make_shared(ros_clock); + auto timer_interface = std::make_shared( + get_node_base_interface(), get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + } +} + +rcl_interfaces::msg::SetParametersResult Lanelet2MapFilterComponent::paramCallback( + const std::vector & p) +{ + if (get_param(p, "voxel_size_x", voxel_size_x_)) { + RCLCPP_DEBUG(get_logger(), "Setting voxel_size_x to: %f.", voxel_size_x_); + } + + if (get_param(p, "voxel_size_y", voxel_size_y_)) { + RCLCPP_DEBUG(get_logger(), "Setting voxel_size_y to: %f.", voxel_size_y_); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} + +bool Lanelet2MapFilterComponent::transformPointCloud( + const std::string & in_target_frame, const PointCloud2ConstPtr & in_cloud_ptr, + PointCloud2 * out_cloud_ptr) +{ + if (in_target_frame == in_cloud_ptr->header.frame_id) { + *out_cloud_ptr = *in_cloud_ptr; + return true; + } + + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer_->lookupTransform( + in_target_frame, in_cloud_ptr->header.frame_id, in_cloud_ptr->header.stamp, + rclcpp::Duration::from_seconds(1.0)); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + return false; + } + // tf2::doTransform(*in_cloud_ptr, *out_cloud_ptr, transform_stamped); + Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(mat, *in_cloud_ptr, *out_cloud_ptr); + out_cloud_ptr->header.frame_id = in_target_frame; + return true; +} + +LinearRing2d Lanelet2MapFilterComponent::getConvexHull( + const pcl::PointCloud::Ptr & input_cloud) +{ + // downsample pointcloud to reduce convex hull calculation cost + pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + downsampled_cloud->points.reserve(input_cloud->points.size()); + pcl::VoxelGrid filter; + filter.setInputCloud(input_cloud); + filter.setLeafSize(0.5, 0.5, 100.0); + filter.filter(*downsampled_cloud); + + MultiPoint2d candidate_points; + for (const auto & p : downsampled_cloud->points) { + candidate_points.emplace_back(p.x, p.y); + } + + LinearRing2d convex_hull; + boost::geometry::convex_hull(candidate_points, convex_hull); + + return convex_hull; +} + +lanelet::ConstLanelets Lanelet2MapFilterComponent::getIntersectedLanelets( + const LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets) +{ + lanelet::ConstLanelets intersected_lanelets; + for (const auto & road_lanelet : road_lanelets) { + if (boost::geometry::intersects(convex_hull, road_lanelet.polygon2d().basicPolygon())) { + intersected_lanelets.push_back(road_lanelet); + } + } + return intersected_lanelets; +} + +bool Lanelet2MapFilterComponent::pointWithinLanelets( + const Point2d & point, const lanelet::ConstLanelets & intersected_lanelets) +{ + for (const auto & lanelet : intersected_lanelets) { + if (boost::geometry::within(point, lanelet.polygon2d().basicPolygon())) { + return true; + } + } + return false; +} + +pcl::PointCloud Lanelet2MapFilterComponent::getLaneFilteredPointCloud( + const lanelet::ConstLanelets & intersected_lanelets, + const pcl::PointCloud::Ptr & cloud) +{ + pcl::PointCloud filtered_cloud; + + filtered_cloud.header = cloud->header; + pcl::PointCloud::Ptr centralized_cloud(new pcl::PointCloud); + centralized_cloud->reserve(cloud->size()); + pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + + // The coordinates of the point cloud are too large, resulting in calculation errors, + // so offset them to the center. + // https://github.com/PointCloudLibrary/pcl/issues/4895 + Eigen::Vector4f centroid; + pcl::compute3DCentroid(*cloud, centroid); + for (const auto & p : cloud->points) { + centralized_cloud->points.push_back( + pcl::PointXYZ(p.x - centroid[0], p.y - centroid[1], p.z - centroid[2])); + } + + pcl::VoxelGrid voxel_grid; + voxel_grid.setLeafSize(voxel_size_x_, voxel_size_y_, 100000.0); + voxel_grid.setInputCloud(centralized_cloud); + voxel_grid.setSaveLeafLayout(true); + voxel_grid.filter(*downsampled_cloud); + + std::unordered_map> downsampled2original_map; + for (const auto & p : centralized_cloud->points) { + if (std::isnan(p.x) || std::isnan(p.y) || std::isnan(p.z)) { + continue; + } + const size_t index = voxel_grid.getCentroidIndex(p); + downsampled2original_map[index].points.push_back(p); + } + + for (auto & point : downsampled_cloud->points) { + if (pointWithinLanelets( + Point2d(point.x + centroid[0], point.y + centroid[1]), intersected_lanelets)) { + const size_t index = voxel_grid.getCentroidIndex(point); + for (auto & original_point : downsampled2original_map[index].points) { + original_point.x += centroid[0]; + original_point.y += centroid[1]; + original_point.z += centroid[2]; + filtered_cloud.points.push_back(original_point); + } + } + } + + return filtered_cloud; +} + +void Lanelet2MapFilterComponent::pointcloudCallback(const PointCloud2ConstPtr cloud_msg) +{ + if (!lanelet_map_ptr_) { + return; + } + // transform pointcloud to map frame + PointCloud2Ptr input_transformed_cloud_ptr(new sensor_msgs::msg::PointCloud2); + if (!transformPointCloud("map", cloud_msg, input_transformed_cloud_ptr.get())) { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), + "Failed transform from " + << "map" + << " to " << cloud_msg->header.frame_id); + return; + } + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromROSMsg(*input_transformed_cloud_ptr, *cloud); + if (cloud->points.empty()) { + return; + } + // calculate convex hull + const auto convex_hull = getConvexHull(cloud); + // get intersected lanelets + lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets(convex_hull, road_lanelets_); + // filter pointcloud by lanelet + const auto filtered_cloud = getLaneFilteredPointCloud(intersected_lanelets, cloud); + // transform pointcloud to input frame + PointCloud2Ptr output_cloud_ptr(new sensor_msgs::msg::PointCloud2); + pcl::toROSMsg(filtered_cloud, *output_cloud_ptr); + PointCloud2Ptr output_transformed_cloud_ptr(new sensor_msgs::msg::PointCloud2); + if (!transformPointCloud( + cloud_msg->header.frame_id, output_cloud_ptr, output_transformed_cloud_ptr.get())) { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), + "Failed transform from " << cloud_msg->header.frame_id << " to " + << output_cloud_ptr->header.frame_id); + return; + } + auto output = std::make_unique(*output_transformed_cloud_ptr); + filtered_pointcloud_pub_->publish(std::move(output)); +} + +void Lanelet2MapFilterComponent::mapCallback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr map_msg) +{ + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_); + const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); +} + +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::Lanelet2MapFilterComponent) diff --git a/sensing/tier4_pcl_extensions/CMakeLists.txt b/sensing/tier4_pcl_extensions/CMakeLists.txt new file mode 100644 index 0000000000000..8eacc49d56d9c --- /dev/null +++ b/sensing/tier4_pcl_extensions/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.5) +project(tier4_pcl_extensions) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +ament_auto_add_library(tier4_pcl_extensions SHARED + src/voxel_grid_nearest_centroid.cpp +) + +target_link_libraries(tier4_pcl_extensions ${PCL_LIBRARIES}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/sensing/tier4_pcl_extensions/README.md b/sensing/tier4_pcl_extensions/README.md new file mode 100644 index 0000000000000..68603795483a3 --- /dev/null +++ b/sensing/tier4_pcl_extensions/README.md @@ -0,0 +1,35 @@ +# tier4_pcl_extensions + +## Purpose + +The `tier4_pcl_extensions` is a pcl extension library. The voxel grid filter in this package works with a different algorithm than the original one. + +## Inner-workings / Algorithms + +### Original Algorithm [1] + +1. create a 3D voxel grid over the input pointcloud data +2. calculate centroid in each voxel +3. all the points are approximated with their centroid + +### Extended Algorithm + +1. create a 3D voxel grid over the input pointcloud data +2. calculate centroid in each voxel +3. **all the points are approximated with the closest point to their centroid** + +## Inputs / Outputs + +## Parameters + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +[1] + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp new file mode 100644 index 0000000000000..d3ae763ffe4bf --- /dev/null +++ b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp @@ -0,0 +1,538 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef TIER4_PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_HPP_ +#define TIER4_PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_HPP_ + +#include +#include +#include +#include + +#include +#include + +namespace pcl +{ +/** \brief A searchable voxel strucure containing the mean and covariance of the data. + * \note For more information please see + * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — + * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. + * PhD thesis, Orebro University. Orebro Studies in Technology 36 + * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) + */ +template +class VoxelGridNearestCentroid : public VoxelGrid +{ +protected: + using VoxelGrid::filter_name_; + using VoxelGrid::getClassName; + using VoxelGrid::input_; + using VoxelGrid::indices_; + using VoxelGrid::filter_limit_negative_; + using VoxelGrid::filter_limit_min_; + using VoxelGrid::filter_limit_max_; + using VoxelGrid::filter_field_name_; + + using VoxelGrid::downsample_all_data_; + using VoxelGrid::leaf_layout_; + using VoxelGrid::save_leaf_layout_; + using VoxelGrid::leaf_size_; + using VoxelGrid::min_b_; + using VoxelGrid::max_b_; + using VoxelGrid::inverse_leaf_size_; + using VoxelGrid::div_b_; + using VoxelGrid::divb_mul_; + + typedef typename pcl::traits::fieldList::type FieldList; + typedef typename Filter::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + +public: + typedef boost::shared_ptr> Ptr; + typedef boost::shared_ptr> ConstPtr; + + /** \brief Simple structure to hold a centroid, covariance and the number of points in a leaf. + * Inverse covariance, eigen vectors and eigen values are precomputed. */ + struct Leaf + { + /** \brief Constructor. + * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref + * evecs_ to the identity matrix + */ + Leaf() + : nr_points(0), + // mean_ (Eigen::Vector3d::Zero ()), + centroid() + // cov_ (Eigen::Matrix3d::Identity ()), + // icov_ (Eigen::Matrix3d::Zero ()), + // evecs_ (Eigen::Matrix3d::Identity ()), + // evals_ (Eigen::Vector3d::Zero ()) + { + } + + /** \brief Get the voxel covariance. + * \return covariance matrix + */ + // Eigen::Matrix3d + // getCov () const + // { + // return (cov_); + // } + + /** \brief Get the inverse of the voxel covariance. + * \return inverse covariance matrix + */ + // Eigen::Matrix3d + // getInverseCov () const + // { + // return (icov_); + // } + + /** \brief Get the voxel centroid. + * \return centroid + */ + // Eigen::Vector3d + // getMean () const + // { + // return (mean_); + // } + + /** \brief Get the eigen vectors of the voxel covariance. + * \note Order corresponds with \ref getEvals + * \return matrix whose columns contain eigen vectors + */ + // Eigen::Matrix3d + // getEvecs () const + // { + // return (evecs_); + // } + + /** \brief Get the eigen values of the voxel covariance. + * \note Order corresponds with \ref getEvecs + * \return vector of eigen values + */ + // Eigen::Vector3d + // getEvals () const + // { + // return (evals_); + // } + + /** \brief Get the number of points contained by this voxel. + * \return number of points + */ + int getPointCount() const { return nr_points; } + + /** \brief Number of points contained by voxel */ + int nr_points; + + /** \brief 3D voxel centroid */ + // Eigen::Vector3d mean_; + + /** \brief Nd voxel centroid + * \note Differs from \ref mean_ when color data is used + */ + Eigen::VectorXf centroid; + + /** \brief Voxel covariance matrix */ + // Eigen::Matrix3d cov_; + + /** \brief Inverse of voxel covariance matrix */ + // Eigen::Matrix3d icov_; + + /** \brief Eigen vectors of voxel covariance matrix */ + // Eigen::Matrix3d evecs_; + + /** \brief Eigen values of voxel covariance matrix */ + // Eigen::Vector3d evals_; + + PointCloud points; + }; + + /** \brief Pointer to VoxelGridNearestCentroid leaf structure */ + typedef Leaf * LeafPtr; + + /** \brief Const pointer to VoxelGridNearestCentroid leaf structure */ + typedef const Leaf * LeafConstPtr; + +public: + /** \brief Constructor. + * Sets \ref leaf_size_ to 0 and \ref searchable_ to false. + */ + VoxelGridNearestCentroid() + : searchable_(true), + // min_points_per_voxel_ (6), + min_points_per_voxel_(1), + // min_covar_eigvalue_mult_ (0.01), + leaves_(), + voxel_centroids_(), + voxel_centroids_leaf_indices_(), + kdtree_() + { + downsample_all_data_ = false; + save_leaf_layout_ = false; + leaf_size_.setZero(); + min_b_.setZero(); + max_b_.setZero(); + filter_name_ = "VoxelGridNearestCentroid"; + } + + /** \brief Set the minimum number of points required for a cell to be used + * (must be 3 or greater for covariance calculation). + * \param[in] min_points_per_voxel the minimum number of points for required + * for a voxel to be used + */ + inline void setMinPointPerVoxel(int min_points_per_voxel) + { + // if(min_points_per_voxel > 2) + if (min_points_per_voxel > 1) { + min_points_per_voxel_ = min_points_per_voxel; + } else { + // PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per + // Voxel to 3 ", this->getClassName ().c_str ()); + // min_points_per_voxel_ = 3; + PCL_WARN( + "%s: Covariance calculation requires at least 1 points, setting Min Point per Voxel to 3 ", + this->getClassName().c_str()); + min_points_per_voxel_ = 1; + } + } + + /** \brief Get the minimum number of points required for a cell to be used. + * \return the minimum number of points for required for a voxel to be used + */ + inline int getMinPointPerVoxel() { return min_points_per_voxel_; } + + /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance + * matrices. \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues + */ + // inline void + // setCovEigValueInflationRatio (double min_covar_eigvalue_mult) + // { + // min_covar_eigvalue_mult_ = min_covar_eigvalue_mult; + // } + + /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance + * matrices. \return the minimum allowable ratio between eigenvalues + */ + // inline double + // getCovEigValueInflationRatio () + // { + // return min_covar_eigvalue_mult_; + // } + + /** \brief Filter cloud and initializes voxel structure. + * \param[out] output cloud containing centroids of voxels containing a sufficient number of + * points \param[in] searchable flag if voxel structure is searchable, if true then kdtree is + * built + */ + inline void filter(PointCloud & output, bool searchable = false) + { + searchable_ = searchable; + applyFilter(output); + + voxel_centroids_ = PointCloudPtr(new PointCloud(output)); + + if (searchable_ && voxel_centroids_->size() > 0) { + // Initiates kdtree of the centroids of voxels containing a sufficient number of points + kdtree_.setInputCloud(voxel_centroids_); + } + } + + /** \brief Initializes voxel structure. + * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built + */ + inline void filter(bool searchable = false) + { + searchable_ = searchable; + voxel_centroids_ = PointCloudPtr(new PointCloud); + applyFilter(*voxel_centroids_); + + if (searchable_ && voxel_centroids_->size() > 0) { + // Initiates kdtree of the centroids of voxels containing a sufficient number of points + kdtree_.setInputCloud(voxel_centroids_); + } + } + + /** \brief Get the voxel containing point p. + * \param[in] index the index of the leaf structure node + * \return const pointer to leaf structure + */ + inline LeafConstPtr getLeaf(int index) + { + typename std::map::iterator leaf_iter = leaves_.find(index); + if (leaf_iter != leaves_.end()) { + LeafConstPtr ret(&(leaf_iter->second)); + return ret; + } else { + return NULL; + } + } + + /** \brief Get the voxel containing point p. + * \param[in] p the point to get the leaf structure at + * \return const pointer to leaf structure + */ + inline LeafConstPtr getLeaf(PointT & p) + { + // Generate index associated with p + int ijk0 = static_cast(floor(p.x * inverse_leaf_size_[0]) - min_b_[0]); + int ijk1 = static_cast(floor(p.y * inverse_leaf_size_[1]) - min_b_[1]); + int ijk2 = static_cast(floor(p.z * inverse_leaf_size_[2]) - min_b_[2]); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + // Find leaf associated with index + typename std::map::iterator leaf_iter = leaves_.find(idx); + if (leaf_iter != leaves_.end()) { + // If such a leaf exists return the pointer to the leaf structure + LeafConstPtr ret(&(leaf_iter->second)); + return ret; + } else { + return NULL; + } + } + + /** \brief Get the voxel containing point p. + * \param[in] p the point to get the leaf structure at + * \return const pointer to leaf structure + */ + inline LeafConstPtr getLeaf(Eigen::Vector3f & p) + { + // Generate index associated with p + int ijk0 = static_cast(floor(p[0] * inverse_leaf_size_[0]) - min_b_[0]); + int ijk1 = static_cast(floor(p[1] * inverse_leaf_size_[1]) - min_b_[1]); + int ijk2 = static_cast(floor(p[2] * inverse_leaf_size_[2]) - min_b_[2]); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + // Find leaf associated with index + typename std::map::iterator leaf_iter = leaves_.find(idx); + if (leaf_iter != leaves_.end()) { + // If such a leaf exists return the pointer to the leaf structure + LeafConstPtr ret(&(leaf_iter->second)); + return ret; + } else { + return NULL; + } + } + + /** \brief Get the voxels surrounding point p, not including the voxel containing point p. + * \note Only voxels containing a sufficient number of points are used (slower than radius search + * in practice). \param[in] reference_point the point to get the leaf structure at \param[out] + * neighbors \return number of neighbors found + */ + // int + // getNeighborhoodAtPoint (const PointT& reference_point, std::vector &neighbors); + + /** \brief Get the leaf structure map + * \return a map containing all leaves + */ + inline const std::map & getLeaves() { return leaves_; } + + /** \brief Get a pointcloud containing the voxel centroids + * \note Only voxels containing a sufficient number of points are used. + * \return a map containing all leaves + */ + inline PointCloudPtr getCentroids() { return voxel_centroids_; } + + /** \brief Get a cloud to visualize each voxels normal distribution. + * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel + */ + // void + // getDisplayCloud (pcl::PointCloud& cell_cloud); + + /** \brief Search for the k-nearest occupied voxels for the given query point. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] point the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \return number of neighbors found + */ + int nearestKSearch( + const PointT & point, int k, std::vector & k_leaves, + std::vector & k_sqr_distances) + { + k_leaves.clear(); + + // Check if kdtree has been built + if (!searchable_) { + PCL_WARN("%s: Not Searchable", this->getClassName().c_str()); + return 0; + } + + // Find k-nearest neighbors in the occupied voxel centroid cloud + std::vector k_indices; + k = kdtree_.nearestKSearch(point, k, k_indices, k_sqr_distances); + + // Find leaves corresponding to neighbors + k_leaves.reserve(k); + for (std::vector::iterator iter = k_indices.begin(); iter != k_indices.end(); iter++) { + k_leaves.push_back(&leaves_[voxel_centroids_leaf_indices_[*iter]]); + } + return k; + } + + /** \brief Search for the k-nearest occupied voxels for the given query point. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] cloud the given query point + * \param[in] index the index + * \param[in] k the number of neighbors to search for + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \return number of neighbors found + */ + inline int nearestKSearch( + const PointCloud & cloud, int index, int k, std::vector & k_leaves, + std::vector & k_sqr_distances) + { + if (index >= static_cast(cloud.points.size()) || index < 0) { + return 0; + } + return nearestKSearch(cloud.points[index], k, k_leaves, k_sqr_distances); + } + + /** \brief Search for all the nearest occupied voxels of the query point in a given radius. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] point the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn + * \return number of neighbors found + */ + int radiusSearch( + const PointT & point, double radius, std::vector & k_leaves, + std::vector & k_sqr_distances, unsigned int max_nn = 0) + { + k_leaves.clear(); + + // Check if kdtree has been built + if (!searchable_) { + PCL_WARN("%s: Not Searchable", this->getClassName().c_str()); + return 0; + } + + // Find neighbors within radius in the occupied voxel centroid cloud + std::vector k_indices; + int k = kdtree_.radiusSearch(point, radius, k_indices, k_sqr_distances, max_nn); + + // Find leaves corresponding to neighbors + k_leaves.reserve(k); + for (std::vector::iterator iter = k_indices.begin(); iter != k_indices.end(); iter++) { + k_leaves.push_back(&leaves_[voxel_centroids_leaf_indices_[*iter]]); + } + return k; + } + + /** \brief Search for all the nearest occupied voxels of the query point in a given radius. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] cloud the given query point + * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn + * \return number of neighbors found + */ + inline int radiusSearch( + const PointCloud & cloud, int index, double radius, std::vector & k_leaves, + std::vector & k_sqr_distances, unsigned int max_nn = 0) + { + if (index >= static_cast(cloud.points.size()) || index < 0) { + return 0; + } + return radiusSearch(cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn); + } + +protected: + /** \brief Filter cloud and initializes voxel structure. + * \param[out] output cloud containing centroids of voxels containing a sufficient + * number of points + */ + void applyFilter(PointCloud & output); + + /** \brief Flag to determine if voxel structure is searchable. */ + bool searchable_; + + /** \brief Minimum points contained with in a voxel to allow it to be useable. */ + int min_points_per_voxel_; + + /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance + * matrices. */ + // double min_covar_eigvalue_mult_; + + /** \brief Voxel structure containing all leaf nodes (includes voxels with less than + * a sufficient number of points). */ + std::map leaves_; + + /** \brief Point cloud containing centroids of voxels containing atleast + * minimum number of points. */ + PointCloudPtr voxel_centroids_; + + /** \brief Indices of leaf structures associated with each point in \ref voxel_centroids_ + * (used for searching). */ + std::vector voxel_centroids_leaf_indices_; + + /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */ + KdTreeFLANN kdtree_; +}; +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +// #include +#include +#endif + +#endif // TIER4_PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_HPP_ diff --git a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp new file mode 100644 index 0000000000000..84ebacf4e57ed --- /dev/null +++ b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp @@ -0,0 +1,345 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef TIER4_PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_IMPL_HPP_ +#define TIER4_PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_IMPL_HPP_ + +#include "tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////// +template +void pcl::VoxelGridNearestCentroid::applyFilter(PointCloud & output) +{ + // Has the input dataset been set already? + if (!input_) { + PCL_WARN("[pcl::%s::applyFilter] No input dataset given!\n", getClassName().c_str()); + output.width = output.height = 0; + output.points.clear(); + return; + } + + // Copy the header (and thus the frame_id) + allocate enough space for points + output.height = 1; // downsampling breaks the organized structure + output.is_dense = true; // we filter out invalid points + output.points.clear(); + + Eigen::Vector4f min_p, max_p; + // Get the minimum and maximum dimensions + if (!filter_field_name_.empty()) { // If we don't want to process the entire cloud... + getMinMax3D( + input_, filter_field_name_, static_cast(filter_limit_min_), + static_cast(filter_limit_max_), min_p, max_p, filter_limit_negative_); + } else { + getMinMax3D(*input_, min_p, max_p); + } + + // Check that the leaf size is not too small, given the size of the data + std::int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0]) + 1; + std::int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1]) + 1; + std::int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2]) + 1; + + if ((dx * dy * dz) > std::numeric_limits::max()) { + PCL_WARN( + "[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would " + "overflow.", // NOLINT + getClassName().c_str()); + output.clear(); + return; + } + + // Compute the minimum and maximum bounding box values + min_b_[0] = static_cast(floor(min_p[0] * inverse_leaf_size_[0])); + max_b_[0] = static_cast(floor(max_p[0] * inverse_leaf_size_[0])); + min_b_[1] = static_cast(floor(min_p[1] * inverse_leaf_size_[1])); + max_b_[1] = static_cast(floor(max_p[1] * inverse_leaf_size_[1])); + min_b_[2] = static_cast(floor(min_p[2] * inverse_leaf_size_[2])); + max_b_[2] = static_cast(floor(max_p[2] * inverse_leaf_size_[2])); + + // Compute the number of divisions needed along all axis + div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones(); + div_b_[3] = 0; + + // Clear the leaves + leaves_.clear(); + + // Set up the division multiplier + divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0); + + int centroid_size = 4; + + if (downsample_all_data_) { + centroid_size = boost::mpl::size::value; + } + + // ---[ RGB special case + std::vector fields; + int rgba_index = -1; + rgba_index = pcl::getFieldIndex("rgb", fields); + if (rgba_index == -1) { + rgba_index = pcl::getFieldIndex("rgba", fields); + } + if (rgba_index >= 0) { + rgba_index = fields[rgba_index].offset; + centroid_size += 3; + } + + // If we don't want to process the entire cloud, but rather filter points far + // away from the viewpoint first... + if (!filter_field_name_.empty()) { + // Get the distance field index + std::vector fields; + int distance_idx = pcl::getFieldIndex(filter_field_name_, fields); + if (distance_idx == -1) { + PCL_WARN( + "[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName().c_str(), + distance_idx); + } + + // First pass: go over all points and insert them into the right leaf + for (size_t cp = 0; cp < input_->points.size(); ++cp) { + if (!input_->is_dense) { + // Check if the point is invalid + if ( + !std::isfinite(input_->points[cp].x) || !std::isfinite(input_->points[cp].y) || + !std::isfinite(input_->points[cp].z)) { + continue; + } + } + + // Get the distance value + const std::uint8_t * pt_data = reinterpret_cast(&input_->points[cp]); + float distance_value = 0; + memcpy(&distance_value, pt_data + fields[distance_idx].offset, sizeof(float)); + + if (filter_limit_negative_) { + // Use a threshold for cutting out points which inside the interval + if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) { + continue; + } + } else { + // Use a threshold for cutting out points which are too close/far away + if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) { + continue; + } + } + + int ijk0 = static_cast( + floor(input_->points[cp].x * inverse_leaf_size_[0]) - static_cast(min_b_[0])); + int ijk1 = static_cast( + floor(input_->points[cp].y * inverse_leaf_size_[1]) - static_cast(min_b_[1])); + int ijk2 = static_cast( + floor(input_->points[cp].z * inverse_leaf_size_[2]) - static_cast(min_b_[2])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + Leaf & leaf = leaves_[idx]; + if (leaf.nr_points == 0) { + leaf.centroid.resize(centroid_size); + leaf.centroid.setZero(); + } + + // Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); + + // Do we need to process all the fields? + if (!downsample_all_data_) { + Eigen::Vector4f pt(input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); + leaf.centroid.template head<4>() += pt; + } else { + // Copy all the fields + Eigen::VectorXf centroid = Eigen::VectorXf::Zero(centroid_size); + // ---[ RGB special case + if (rgba_index >= 0) { + // fill r/g/b data + int rgb; + memcpy( + &rgb, reinterpret_cast(&input_->points[cp]) + rgba_index, sizeof(int)); + centroid[centroid_size - 3] = static_cast((rgb >> 16) & 0x0000ff); + centroid[centroid_size - 2] = static_cast((rgb >> 8) & 0x0000ff); + centroid[centroid_size - 1] = static_cast((rgb)&0x0000ff); + } + pcl::for_each_type( + NdCopyPointEigenFunctor(input_->points[cp], centroid)); + leaf.centroid += centroid; + } + ++leaf.nr_points; + } + } else { // No distance filtering, process all data + // First pass: go over all points and insert them into the right leaf + for (size_t cp = 0; cp < input_->points.size(); ++cp) { + if (!input_->is_dense) { + // Check if the point is invalid + if ( + !std::isfinite(input_->points[cp].x) || !std::isfinite(input_->points[cp].y) || + !std::isfinite(input_->points[cp].z)) { + continue; + } + } + + int ijk0 = static_cast( + floor(input_->points[cp].x * inverse_leaf_size_[0]) - static_cast(min_b_[0])); + int ijk1 = static_cast( + floor(input_->points[cp].y * inverse_leaf_size_[1]) - static_cast(min_b_[1])); + int ijk2 = static_cast( + floor(input_->points[cp].z * inverse_leaf_size_[2]) - static_cast(min_b_[2])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + Leaf & leaf = leaves_[idx]; + if (leaf.nr_points == 0) { + leaf.centroid.resize(centroid_size); + leaf.centroid.setZero(); + } + + // Do we need to process all the fields? + if (!downsample_all_data_) { + Eigen::Vector4f pt(input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); + leaf.centroid.template head<4>() += pt; + } else { + // Copy all the fields + Eigen::VectorXf centroid = Eigen::VectorXf::Zero(centroid_size); + // ---[ RGB special case + if (rgba_index >= 0) { + // Fill r/g/b data, assuming that the order is BGRA + int rgb; + memcpy( + &rgb, reinterpret_cast(&input_->points[cp]) + rgba_index, sizeof(int)); + centroid[centroid_size - 3] = static_cast((rgb >> 16) & 0x0000ff); + centroid[centroid_size - 2] = static_cast((rgb >> 8) & 0x0000ff); + centroid[centroid_size - 1] = static_cast((rgb)&0x0000ff); + } + pcl::for_each_type( + NdCopyPointEigenFunctor(input_->points[cp], centroid)); + leaf.centroid += centroid; + } + leaf.points.push_back(input_->points[cp]); + ++leaf.nr_points; + } + } + + // Second pass: go over all leaves and compute centroids and covariance matrices + output.points.reserve(leaves_.size()); + if (searchable_) { + voxel_centroids_leaf_indices_.reserve(leaves_.size()); + } + int cp = 0; + if (save_leaf_layout_) { + leaf_layout_.resize(div_b_[0] * div_b_[1] * div_b_[2], -1); + } + + // Eigen values and vectors calculated to prevent near singular matrices + // Eigen::SelfAdjointEigenSolver eigensolver; + // Eigen::Matrix3d eigen_val; + // Eigen::Vector3d pt_sum; + + // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the + // max eigen value. + // double min_covar_eigvalue; + + for (typename std::map::iterator it = leaves_.begin(); it != leaves_.end(); ++it) { + // Normalize the centroid + Leaf & leaf = it->second; + + // Normalize the centroid + leaf.centroid /= static_cast(leaf.nr_points); + // Point sum used for single pass covariance calculation + // pt_sum = leaf.mean_; + // Normalize mean + // leaf.mean_ /= leaf.nr_points; + + // If the voxel contains sufficient points, its covariance is calculated and is added to the + // voxel centroids and output clouds. + // Points with less than the minimum points will have a can not be accurately approximated + // using a normal distribution. + if (leaf.nr_points >= min_points_per_voxel_) { + if (save_leaf_layout_) { + leaf_layout_[it->first] = cp++; + } + + output.push_back(PointT()); + + // TODO(kenji.miyake) refactor + double dis_min = 1000000.0; + size_t i_min = 0; + for (size_t i = 0; i < leaf.points.size(); ++i) { + double dis = std::pow(leaf.points.at(i).x - leaf.centroid[0], 2.0) + + std::pow(leaf.points.at(i).y - leaf.centroid[1], 2.0) + + std::pow(leaf.points.at(i).z - leaf.centroid[2], 2.0); + if (dis < dis_min) { + dis_min = dis; + i_min = i; + } + } + output.points.back() = leaf.points.at(i_min); + + // Stores the voxel indices for fast access searching + if (searchable_) { + voxel_centroids_leaf_indices_.push_back(static_cast(it->first)); + } + } + } + output.width = static_cast(output.points.size()); +} + +#define PCL_INSTANTIATE_VoxelGridNearestCentroid(T) \ + template class PCL_EXPORTS pcl::VoxelGridNearestCentroid; + +#endif // TIER4_PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_IMPL_HPP_ diff --git a/sensing/tier4_pcl_extensions/package.xml b/sensing/tier4_pcl_extensions/package.xml new file mode 100644 index 0000000000000..0b6518b5793ff --- /dev/null +++ b/sensing/tier4_pcl_extensions/package.xml @@ -0,0 +1,24 @@ + + + tier4_pcl_extensions + 0.1.0 + The tier4_pcl_extensions package + Ryu Yamamoto + Apache License 2.0 + ament_cmake_auto + eigen3_cmake_module + + eigen3_cmake_module + + eigen + eigen + + libpcl-all-dev + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/sensing/tier4_pcl_extensions/src/voxel_grid_nearest_centroid.cpp b/sensing/tier4_pcl_extensions/src/voxel_grid_nearest_centroid.cpp new file mode 100644 index 0000000000000..52bbe7f58b426 --- /dev/null +++ b/sensing/tier4_pcl_extensions/src/voxel_grid_nearest_centroid.cpp @@ -0,0 +1,64 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp" + +#include + +#ifndef PCL_NO_PRECOMPILE +#include + +#include + +// Instantiations of specific point types +PCL_INSTANTIATE(VoxelGridNearestCentroid, PCL_XYZ_POINT_TYPES) + +#endif // PCL_NO_PRECOMPILE