Merge branch 'tier4/proposal' into 1-update-autoware_external_api_msgs

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

perception/occupancy_grid_map_outlier_filter/CMakeLists.txt

@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 3.5)
+project(occupancy_grid_map_outlier_filter)
+
+### 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(occupancy_grid_map_outlier_filter SHARED
+  src/occupancy_grid_map_outlier_filter_nodelet.cpp
+)
+
+target_link_libraries(occupancy_grid_map_outlier_filter
+  ${Boost_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+
+if(OPENMP_FOUND)
+  set_target_properties(occupancy_grid_map_outlier_filter PROPERTIES
+    COMPILE_FLAGS ${OpenMP_CXX_FLAGS}
+    LINK_FLAGS ${OpenMP_CXX_FLAGS}
+  )
+endif()
+
+
+# -- Occupancy Grid Map Outlier Filter --
+rclcpp_components_register_node(occupancy_grid_map_outlier_filter
+  PLUGIN "occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent"
+  EXECUTABLE occupancy_grid_map_outlier_filter_node)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+#############
+## Install ##
+#############
+
+ament_auto_package(INSTALL_TO_SHARE)

perception/occupancy_grid_map_outlier_filter/README.md

@@ -0,0 +1,63 @@
+# occupancy_grid_map_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
+
+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)
+
+![occupancy_grid_map_outlier_filter](./image/occupancy_grid_map_outlier_filter.drawio.svg)
+
+## 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
+
+## (Optional) Future extensions / Unimplemented parts

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp → perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp

@@ -12,8 +12,10 @@
 // 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_
+#ifndef OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_
+#define OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_
+
+#include "pointcloud_preprocessor/filter.hpp"
 
 #include <pcl/common/impl/common.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -36,7 +38,7 @@
 #include <memory>
 #include <string>
 
-namespace pointcloud_preprocessor
+namespace occupancy_grid_map_outlier_filter
 {
 using geometry_msgs::msg::Pose;
 using nav_msgs::msg::OccupancyGrid;
@@ -121,6 +123,6 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node
   std::string base_link_frame_;
   int cost_threshold_;
 };
-}  // namespace pointcloud_preprocessor
+}  // namespace occupancy_grid_map_outlier_filter
 
-#endif  // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_
+#endif  // OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_

perception/occupancy_grid_map_outlier_filter/package.xml

@@ -0,0 +1,51 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>occupancy_grid_map_outlier_filter</name>
+  <version>0.1.0</version>
+  <description>The ROS2 occupancy_grid_map_outlier_filter package</description>
+
+  <maintainer email="abrahammonrroy@yahoo.com">amc-nu</maintainer>
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author>Open Perception</author>
+  <author email="julius@kammerl.de">Julius Kammerl</author>
+  <author email="william@osrfoundation.org">William Woodall</author>
+  <maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
+  <maintainer email="www.kentaro.wada@gmail.com">Kentaro Wada</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_debug_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>image_transport</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>libopencv-dev</depend>
+  <depend>libpcl-all-dev</depend>
+  <depend>message_filters</depend>
+  <depend>nav_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_msgs</depend>
+  <depend>pcl_ros</depend>
+  <depend>pointcloud_preprocessor</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_pcl_extensions</depend>
+  <depend>vehicle_info_util</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

sensing/pointcloud_preprocessor/src/outlier_filter/occupancy_grid_map_outlier_filter_nodelet.cpp → perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

@@ -12,7 +12,7 @@
 // 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 "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp"
 
 #include <autoware_utils/autoware_utils.hpp>
 #include <pcl_ros/transforms.hpp>
@@ -42,8 +42,7 @@ bool transformPointcloud(
       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());
+      rclcpp::get_logger("occupancy_grid_map_outlier_filter"), clock, 5000, "%s", ex.what());
     return false;
   }
   // transform pointcloud
@@ -65,8 +64,7 @@ geometry_msgs::msg::PoseStamped getPoseStamped(
       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());
+      rclcpp::get_logger("occupancy_grid_map_outlier_filter"), clock, 5000, "%s", ex.what());
   }
   return autoware_utils::transform2pose(tf_stamped);
 }
@@ -96,7 +94,7 @@ boost::optional<char> getCost(
 
 }  // namespace
 
-namespace pointcloud_preprocessor
+namespace occupancy_grid_map_outlier_filter
 {
 RadiusSearch2dfilter::RadiusSearch2dfilter(rclcpp::Node & node)
 {
@@ -319,7 +317,8 @@ void OccupancyGridMapOutlierFilterComponent::Debugger::transformToBaseLink(
   transformPointcloud(ros_input, *(node_.tf2_), node_.base_link_frame_, output);
 }
 
-}  // namespace pointcloud_preprocessor
+}  // namespace occupancy_grid_map_outlier_filter
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::OccupancyGridMapOutlierFilterComponent)
+RCLCPP_COMPONENTS_REGISTER_NODE(
+  occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent)

planning/motion_velocity_smoother/scripts/closest_velocity_checker.py

@@ -23,6 +23,7 @@
 from autoware_auto_vehicle_msgs.msg import Engage
 from autoware_auto_vehicle_msgs.msg import VelocityReport
 from autoware_debug_msgs.msg import Float32MultiArrayStamped
+from autoware_debug_msgs.msg import Float32Stamped
 from autoware_planning_msgs.msg import VelocityLimit
 from geometry_msgs.msg import Pose
 from nav_msgs.msg import Odometry
@@ -57,6 +58,7 @@ def __init__(self):
         self.vehicle_engage = None
         self.external_v_lim = np.nan
         self.localization_twist_vx = np.nan
+        self.distance_to_stopline = np.nan
         self.vehicle_twist_vx = np.nan
         self.self_pose = Pose()
         self.data_arr = [np.nan] * DATA_NUM
@@ -132,6 +134,14 @@ def __init__(self):
             VelocityReport, "/vehicle/status/velocity_status", self.CallBackVehicleTwist, 1
         )
 
+        # distance_to_stopline
+        self.pub12 = self.create_subscription(
+            Float32Stamped,
+            scenario + "/motion_velocity_smoother/distance_to_stopline",
+            self.CallBackDistanceToStopline,
+            1,
+        )
+
         # publish data
         self.pub_v_arr = self.create_publisher(Float32MultiArrayStamped, "closest_speeds", 1)
 
@@ -147,9 +157,10 @@ def printInfo(self):
             self.get_logger().info(
                 "| Map Limit | Behavior | Obs Avoid | Obs Stop | External Lim | LatAcc Filtered "
                 "| Optimized | Control VelCmd | Control AccCmd | Vehicle VelCmd | Vehicle AccCmd "
-                "| AW Engage | VC Engage | Localization Vel | Vehicle Vel | [km/h]"
+                "| AW Engage | VC Engage | Localization Vel | Vehicle Vel | [km/h] | Distance [m] "
             )  # noqa: E501
         mps2kmph = 3.6
+        distance_to_stopline = self.distance_to_stopline
         vel_map_lim = self.data_arr[LANE_CHANGE] * mps2kmph
         vel_behavior = self.data_arr[BEHAVIOR_VELOCITY] * mps2kmph
         vel_obs_avoid = self.data_arr[OBSTACLE_AVOID] * mps2kmph
@@ -176,7 +187,7 @@ def printInfo(self):
         self.get_logger().info(
             "| {0: 9.2f} | {1: 8.2f} | {2: 9.2f} | {3: 8.2f} | {4: 12.2f} "
             "| {5: 15.2f} | {6: 9.2f} | {7: 14.2f} | {8: 14.2f} | {9: 14.2f} | {10: 14.2f} "
-            "| {11:>9s} | {12:>9s} | {13: 16.2f} | {14: 11.2f} |".format(  # noqa: E501
+            "| {11:>9s} | {12:>9s} | {13: 16.2f} | {14: 11.2f} |        | {15: 10.2f}".format(  # noqa: E501
                 vel_map_lim,
                 vel_behavior,
                 vel_obs_avoid,
@@ -192,6 +203,7 @@ def printInfo(self):
                 vehicle_engage,
                 vel_localization,
                 vel_vehicle,
+                distance_to_stopline,
             )
         )
         self.count += 1
@@ -217,6 +229,9 @@ def CallBackLocalizationTwist(self, msg):
     def CallBackVehicleTwist(self, msg):
         self.vehicle_twist_vx = msg.longitudinal_velocity
 
+    def CallBackDistanceToStopline(self, msg):
+        self.distance_to_stopline = msg.data
+
     def CallBackBehaviorPathWLid(self, msg):
         # self.get_logger().info('LANE_CHANGE called')
         closest = self.calcClosestPathWLid(msg)

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -49,7 +49,6 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   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
@@ -115,11 +114,6 @@ 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"