Merge pull request autowarefoundation#617 from tier4/sync-upstream

chore: sync upstream

launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml

@@ -10,6 +10,8 @@
   <arg name="container_name" default="occupancy_grid_map_container"/>
   <arg name="occupancy_grid_map_method" description="options: pointcloud_based_occupancy_grid_map, laserscan_based_occupancy_grid_map"/>
   <arg name="occupancy_grid_map_param_path"/>
+  <arg name="occupancy_grid_map_updater" description="options: binary_bayes_filter"/>
+  <arg name="occupancy_grid_map_updater_param_path"/>
 
   <!--pointcloud based method-->
   <group if="$(eval &quot;'$(var occupancy_grid_map_method)'=='pointcloud_based_occupancy_grid_map'&quot;)">
@@ -22,6 +24,8 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="param_file" value="$(var occupancy_grid_map_param_path)"/>
+      <arg name="updater_type" value="$(var occupancy_grid_map_updater)"/>
+      <arg name="updater_param_file" value="$(var occupancy_grid_map_updater_param_path)"/>
     </include>
   </group>
 
@@ -36,6 +40,8 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="param_file" value="$(var occupancy_grid_map_param_path)"/>
+      <arg name="updater_type" value="$(var occupancy_grid_map_updater)"/>
+      <arg name="updater_param_file" value="$(var occupancy_grid_map_updater_param_path)"/>
     </include>
   </group>
 </launch>

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -9,6 +9,8 @@
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
   <arg name="occupancy_grid_map_method"/>
   <arg name="occupancy_grid_map_param_path"/>
+  <arg name="occupancy_grid_map_updater"/>
+  <arg name="occupancy_grid_map_updater_param_path"/>
 
   <!-- centerpoint model configs -->
   <arg name="centerpoint_model_name" default="centerpoint_tiny" description="options: `centerpoint` or `centerpoint_tiny`"/>
@@ -84,6 +86,8 @@
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
         <arg name="occupancy_grid_map_method" value="$(var occupancy_grid_map_method)"/>
         <arg name="occupancy_grid_map_param_path" value="$(var occupancy_grid_map_param_path)"/>
+        <arg name="occupancy_grid_map_updater" value="$(var occupancy_grid_map_updater)"/>
+        <arg name="occupancy_grid_map_updater_param_path" value="$(var occupancy_grid_map_updater_param_path)"/>
       </include>
     </group>
 

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -3,6 +3,8 @@
   <arg name="fault_injection_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
   <arg name="laserscan_based_occupancy_grid_map_param_path"/>
+  <arg name="occupancy_grid_map_updater"/>
+  <arg name="occupancy_grid_map_updater_param_path"/>
   <arg name="pose_initializer_param_path"/>
   <arg name="pose_initializer_common_param_path"/>
 
@@ -49,6 +51,8 @@
       <arg name="output" value="/perception/occupancy_grid_map/map"/>
       <arg name="occupancy_grid_map_method" value="laserscan_based_occupancy_grid_map"/>
       <arg name="occupancy_grid_map_param_path" value="$(var laserscan_based_occupancy_grid_map_param_path)"/>
+      <arg name="occupancy_grid_map_updater" value="$(var occupancy_grid_map_updater)"/>
+      <arg name="occupancy_grid_map_updater_param_path" value="$(var occupancy_grid_map_updater_param_path)"/>
     </include>
   </group>
 

perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    binary_bayes_filter:
+      probability_matrix:
+        occupied_to_occupied: 0.95
+        occupied_to_free: 0.05
+        free_to_occupied: 0.2
+        free_to_free: 0.8
+      v_ratio: 10.0

perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml

@@ -7,7 +7,11 @@
     # ray-tracing center: main sensor frame is preferable like: "velodyne_top"
     scan_origin_frame: "base_link"
 
-    use_height_filter: true
+    height_filter:
+      use_height_filter: true
+      min_height: -1.0
+      max_height: 2.0
+
     enable_single_frame_mode: false
     map_length: 100.0
     map_width: 100.0

perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml

@@ -3,7 +3,11 @@
     map_length: 100.0     # [m]
     map_resolution: 0.5 # [m]
 
-    use_height_filter: true
+    height_filter:
+      use_height_filter: true
+      min_height: -1.0
+      max_height: 2.0
+
     enable_single_frame_mode: false
     # use sensor pointcloud to filter obstacle pointcloud
     filter_obstacle_pointcloud_by_raw_pointcloud: false

perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp

@@ -95,6 +95,8 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node
   std::string gridmap_origin_frame_;
   std::string scan_origin_frame_;
   bool use_height_filter_;
+  double min_height_;
+  double max_height_;
   bool enable_single_frame_mode_;
 };
 

perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp

@@ -85,6 +85,8 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node
   std::string gridmap_origin_frame_;
   std::string scan_origin_frame_;
   bool use_height_filter_;
+  double min_height_;
+  double max_height_;
   bool enable_single_frame_mode_;
   bool filter_obstacle_pointcloud_by_raw_pointcloud_;
 };

perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp

@@ -27,20 +27,14 @@ 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);
-  }
+    const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution);
   bool update(const Costmap2D & single_frame_occupancy_grid_map) override;
+  void initRosParam(rclcpp::Node & node) override;
 
 private:
   inline unsigned char applyBBF(const unsigned char & z, const unsigned char & o);
   Eigen::Matrix2f probability_matrix_;
+  double v_ratio_;
 };
 
 }  // namespace costmap_2d

perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp

@@ -18,6 +18,7 @@
 #include "cost_value.hpp"
 
 #include <nav2_costmap_2d/costmap_2d.hpp>
+#include <rclcpp/node.hpp>
 
 namespace costmap_2d
 {
@@ -32,6 +33,7 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D
   }
   virtual ~OccupancyGridMapUpdaterInterface() = default;
   virtual bool update(const Costmap2D & single_frame_occupancy_grid_map) = 0;
+  virtual void initRosParam(rclcpp::Node & node) = 0;
 };
 
 }  // namespace costmap_2d

perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py

@@ -26,35 +26,34 @@
 import yaml
 
 
-def overwrite_config(param_dict, launch_config_name, node_params_name, context):
-    if LaunchConfiguration(launch_config_name).perform(context) != "":
-        param_dict[node_params_name] = LaunchConfiguration(launch_config_name).perform(context)
-
-
 def launch_setup(context, *args, **kwargs):
     # load parameter files
     param_file = LaunchConfiguration("param_file").perform(context)
     with open(param_file, "r") as f:
         laserscan_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"]["ros__parameters"]
-    overwrite_config(
-        laserscan_based_occupancy_grid_map_node_params,
-        "map_origin",
-        "gridmap_origin_frame",
-        context,
-    )
-    overwrite_config(
-        laserscan_based_occupancy_grid_map_node_params, "scan_origin", "scan_origin_frame", context
-    )
+
+    updater_param_file = LaunchConfiguration("updater_param_file").perform(context)
+    with open(updater_param_file, "r") as f:
+        occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"]
 
     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")),
+                (
+                    "~/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")),
             ],
@@ -90,12 +89,19 @@ def launch_setup(context, *args, **kwargs):
             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")),
+                (
+                    "~/input/obstacle_pointcloud",
+                    LaunchConfiguration("input/obstacle_pointcloud"),
+                ),
+                (
+                    "~/input/raw_pointcloud",
+                    LaunchConfiguration("input/raw_pointcloud"),
+                ),
                 ("~/output/occupancy_grid_map", LaunchConfiguration("output")),
             ],
             parameters=[
                 laserscan_based_occupancy_grid_map_node_params,
+                occupancy_grid_map_updater_params,
                 {
                     "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"),
                     "input_obstacle_and_raw_pointcloud": LaunchConfiguration(
@@ -148,24 +154,27 @@ def add_launch_arg(name: str, default_value=None):
             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("map_origin", "base_link"),
-            add_launch_arg("sensor_origin", "base_link"),
             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"),
             add_launch_arg(
                 "param_file",
                 get_package_share_directory("probabilistic_occupancy_grid_map")
                 + "/config/laserscan_based_occupancy_grid_map.param.yaml",
             ),
+            add_launch_arg(
+                "updater_type",
+                "binary_bayes_filter",
+            ),
+            add_launch_arg(
+                "updater_param_file",
+                get_package_share_directory("probabilistic_occupancy_grid_map")
+                + "/config/updater.param.yaml",
+            ),
             add_launch_arg("use_pointcloud_container", "false"),
             add_launch_arg("container_name", "occupancy_grid_map_container"),
-            add_launch_arg("map_origin", ""),
-            add_launch_arg("scan_origin", ""),
             set_container_executable,
             set_container_mt_executable,
         ]

perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py

@@ -38,27 +38,31 @@ def launch_setup(context, *args, **kwargs):
         pointcloud_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"][
             "ros__parameters"
         ]
-    overwrite_config(
-        pointcloud_based_occupancy_grid_map_node_params,
-        "map_origin",
-        "gridmap_origin_frame",
-        context,
-    )
-    overwrite_config(
-        pointcloud_based_occupancy_grid_map_node_params, "scan_origin", "scan_origin_frame", context
-    )
+
+    updater_param_file = LaunchConfiguration("updater_param_file").perform(context)
+    with open(updater_param_file, "r") as f:
+        occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"]
 
     composable_nodes = [
         ComposableNode(
             package="probabilistic_occupancy_grid_map",
             plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode",
             name="occupancy_grid_map_node",
             remappings=[
-                ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
-                ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")),
+                (
+                    "~/input/obstacle_pointcloud",
+                    LaunchConfiguration("input/obstacle_pointcloud"),
+                ),
+                (
+                    "~/input/raw_pointcloud",
+                    LaunchConfiguration("input/raw_pointcloud"),
+                ),
                 ("~/output/occupancy_grid_map", LaunchConfiguration("output")),
             ],
-            parameters=[pointcloud_based_occupancy_grid_map_node_params],
+            parameters=[
+                pointcloud_based_occupancy_grid_map_node_params,
+                occupancy_grid_map_updater_params,
+            ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         ),
     ]
@@ -112,8 +116,15 @@ def add_launch_arg(name: str, default_value=None):
                 get_package_share_directory("probabilistic_occupancy_grid_map")
                 + "/config/pointcloud_based_occupancy_grid_map.param.yaml",
             ),
-            add_launch_arg("map_origin", ""),
-            add_launch_arg("scan_origin", ""),
+            add_launch_arg(
+                "updater_type",
+                "binary_bayes_filter",
+            ),
+            add_launch_arg(
+                "updater_param_file",
+                get_package_share_directory("probabilistic_occupancy_grid_map")
+                + "/config/updater.param.yaml",
+            ),
             set_container_executable,
             set_container_mt_executable,
         ]

perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp

@@ -51,18 +51,20 @@ LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode(
   using std::placeholders::_3;
 
   /* params */
-  map_frame_ = declare_parameter("map_frame", "map");
-  base_link_frame_ = declare_parameter("base_link_frame", "base_link");
-  gridmap_origin_frame_ = declare_parameter("gridmap_origin_frame", "base_link");
-  scan_origin_frame_ = declare_parameter("scan_origin_frame", "base_link");
-  use_height_filter_ = declare_parameter("use_height_filter", true);
-  enable_single_frame_mode_ = declare_parameter("enable_single_frame_mode", false);
-  const double map_length{declare_parameter("map_length", 100.0)};
-  const double map_width{declare_parameter("map_width", 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)};
+  map_frame_ = this->declare_parameter<std::string>("map_frame");
+  base_link_frame_ = this->declare_parameter<std::string>("base_link_frame");
+  gridmap_origin_frame_ = this->declare_parameter<std::string>("gridmap_origin_frame");
+  scan_origin_frame_ = this->declare_parameter<std::string>("scan_origin_frame");
+  use_height_filter_ = this->declare_parameter<bool>("height_filter.use_height_filter");
+  min_height_ = this->declare_parameter<double>("height_filter.min_height");
+  max_height_ = this->declare_parameter<double>("height_filter.max_height");
+  enable_single_frame_mode_ = this->declare_parameter<bool>("enable_single_frame_mode");
+  const double map_length = this->declare_parameter<double>("map_length");
+  const double map_width = this->declare_parameter<double>("map_width");
+  const double map_resolution = this->declare_parameter<double>("map_resolution");
+  const bool input_obstacle_pointcloud = this->declare_parameter<bool>("input_obstacle_pointcloud");
+  const bool input_obstacle_and_raw_pointcloud =
+    this->declare_parameter<bool>("input_obstacle_and_raw_pointcloud");
 
   /* Subscriber and publisher */
   laserscan_sub_.subscribe(
@@ -90,9 +92,19 @@ LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode(
     _3));
   occupancy_grid_map_pub_ = create_publisher<OccupancyGrid>("~/output/occupancy_grid_map", 1);
 
-  /* Occupancy grid */
-  occupancy_grid_map_updater_ptr_ = std::make_shared<OccupancyGridMapBBFUpdater>(
-    map_length / map_resolution, map_width / map_resolution, map_resolution);
+  const std::string updater_type = this->declare_parameter<std::string>("updater_type");
+  if (updater_type == "binary_bayes_filter") {
+    occupancy_grid_map_updater_ptr_ = std::make_shared<OccupancyGridMapBBFUpdater>(
+      map_length / map_resolution, map_width / map_resolution, map_resolution);
+  } else {
+    RCLCPP_WARN(
+      get_logger(),
+      "specified occupancy grid map updater type [%s] is not found, use binary_bayes_filter",
+      updater_type.c_str());
+    occupancy_grid_map_updater_ptr_ = std::make_shared<OccupancyGridMapBBFUpdater>(
+      map_length / map_resolution, map_width / map_resolution, map_resolution);
+  }
+  occupancy_grid_map_updater_ptr_->initRosParam(*this);
 }
 
 PointCloud2::SharedPtr LaserscanBasedOccupancyGridMapNode::convertLaserscanToPointCLoud2(
@@ -132,14 +144,13 @@ void LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRa
   PointCloud2 cropped_obstacle_pc{};
   PointCloud2 cropped_raw_pc{};
   if (use_height_filter_) {
-    constexpr float min_height = -1.0, max_height = 2.0;
     if (!utils::cropPointcloudByHeight(
-          *input_obstacle_msg, *tf2_, base_link_frame_, min_height, max_height,
+          *input_obstacle_msg, *tf2_, base_link_frame_, min_height_, max_height_,
           cropped_obstacle_pc)) {
       return;
     }
     if (!utils::cropPointcloudByHeight(
-          *input_raw_msg, *tf2_, base_link_frame_, min_height, max_height, cropped_raw_pc)) {
+          *input_raw_msg, *tf2_, base_link_frame_, min_height_, max_height_, cropped_raw_pc)) {
       return;
     }
   }