Use just one pipeline generator+filter definition file

mrpt_pf_localization/src/mrpt_pf_localization_node.cpp

@@ -22,12 +22,27 @@
 #include <mrpt/ros2bridge/time.h>
 #include <mrpt/serialization/CSerializable.h>
 #include <mrpt/system/COutputLogger.h>
+#include <mrpt/version.h>
 #include <pose_cov_ops/pose_cov_ops.h>
 
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <mrpt_msgs_bridge/beacon.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
+#if MRPT_VERSION >= 0x020b08
+#include <mrpt/system/hyperlink.h>
+#else
+namespace mrpt::system	// backwards compatibility:
+{
+std::string hyperlink(
+	const std::string& text, const std::string& uri, bool force_format = false,
+	bool show_uri_anyway = false)
+{
+	return text;
+}
+}  // namespace mrpt::system
+#endif
+
 int main(int argc, char** argv)
 {
 	rclcpp::init(argc, argv);
@@ -40,6 +55,7 @@ int main(int argc, char** argv)
 PFLocalizationNode::PFLocalizationNode(const rclcpp::NodeOptions& options)
 	: rclcpp::Node("mrpt_pf_localization_node", options)
 {
+	using namespace std::string_literals;
 	using std::placeholders::_1;
 
 	// Redirect MRPT logger to ROS logger:
@@ -123,8 +139,11 @@ PFLocalizationNode::PFLocalizationNode(const rclcpp::NodeOptions& options)
 
 	ASSERTMSG_(
 		numSensors > 0,
-		"At least one sensor input source must be defined! Refer to the "
-		"package documentation.");
+		"At least one sensor input source must be defined! Refer to the "s +
+			mrpt::system::hyperlink(
+				"package documentation.",
+				"https://github.com/mrpt-ros-pkg/mrpt_navigation",
+				true /*force ^format*/));
 
 	pubParticles_ = this->create_publisher<geometry_msgs::msg::PoseArray>(
 		nodeParams_.pub_topic_particles, 1);
@@ -239,6 +258,8 @@ void PFLocalizationNode::reload_params_from_ros()
 
 	core_.init_from_yaml(paramsBlock);
 	nodeParams_.loadFrom(paramsBlock);
+
+	RCLCPP_DEBUG_STREAM(get_logger(), paramsBlock);
 }
 
 void PFLocalizationNode::loop()
@@ -784,10 +805,6 @@ void PFLocalizationNode::useROSLogLevel()
 void PFLocalizationNode::NodeParameters::loadFrom(
 	const mrpt::containers::yaml& cfg)
 {
-#if 1
-	cfg.printAsYAML();
-#endif
-
 	MCP_LOAD_OPT(cfg, rate_hz);
 	MCP_LOAD_OPT(cfg, transform_tolerance);
 	MCP_LOAD_OPT(cfg, no_update_tolerance);

mrpt_pointcloud_pipeline/include/mrpt_pointcloud_pipeline/mrpt_pointcloud_pipeline_node.h

@@ -9,6 +9,7 @@
 /* mrpt deps*/
 #include <mp2p_icp/icp_pipeline_from_yaml.h>
 #include <mp2p_icp_filters/FilterDecimateVoxels.h>
+#include <mp2p_icp_filters/Generator.h>
 #include <mrpt/config/CConfigFile.h>
 #include <mrpt/containers/yaml.h>
 #include <mrpt/gui/CDisplayWindow3D.h>
@@ -93,7 +94,6 @@ class LocalObstaclesNode : public rclcpp::Node
 		// Error handling: check if lstSources is empty
 		if (lstSources.empty())
 		{
-			RCLCPP_ERROR(this->get_logger(), "List of topics is empty.");
 			return 0;  // Return early with 0 subscriptions
 		}
 		for (const auto& source : lstSources)
@@ -118,8 +118,6 @@ class LocalObstaclesNode : public rclcpp::Node
 	bool m_one_observation_per_topic = false;
 	std::string m_frameid_reference = "odom";  //!< type:"odom"
 	std::string m_frameid_robot = "base_link";	//!< type: "base_link"
-	std::string m_topic_local_map_pointcloud =
-		"local_map_pointcloud";	 //!< Default: "local_map_pointcloud"
 	std::string m_topics_source_2dscan =
 		"scan, laser1";	 //!< Default: "scan, laser1"
 	std::string m_topics_source_pointclouds = "";
@@ -145,23 +143,27 @@ class LocalObstaclesNode : public rclcpp::Node
 	obs_list_t m_hist_obs;
 	std::mutex m_hist_obs_mtx;	//!< mutex
 
-	CSimplePointsMap::Ptr m_localmap_pts = CSimplePointsMap::Create();
-
 	mrpt::gui::CDisplayWindow3D::Ptr m_gui_win;
 	bool m_visible_raw = true, m_visible_output = true;
 
 	/// Used for example to run voxel grid decimation, etc.
 	/// Refer to mp2p_icp docs
+	std::string m_pipeline_yaml_file;
 	mp2p_icp_filters::FilterPipeline m_per_obs_pipeline, m_final_pipeline;
-	std::string m_filter_output_layer_name;	 //!< mp2p_icp output layer name
-	std::string m_per_obs_filter_yaml_file, m_final_filter_yaml_file;
+	mp2p_icp_filters::GeneratorSet m_generator;
+
+	struct LayerTopicNames
+	{
+		std::string layer;
+		std::string topic;
+		rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub;
+	};
+	std::vector<LayerTopicNames> layer2topic_;
 
 	/**
 	 * @name ROS2 pubs/subs
 	 * @{
 	 */
-	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr
-		m_pub_local_map_pointcloud;
 	std::vector<rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr>
 		m_subs_2dlaser;
 	std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>

mrpt_pointcloud_pipeline/launch/pointcloud_pipeline.launch.py

@@ -40,23 +40,20 @@ def generate_launch_description():
         'one_observation_per_topic',
         default_value='false'
     )
-    per_obs_filter_yaml_file_arg = DeclareLaunchArgument(
-        'per_obs_filter_yaml_file',
+    pipeline_yaml_file_arg = DeclareLaunchArgument(
+        'pipeline_yaml_file',
         default_value=os.path.join(
-            myPkgDir, 'params', 'per-observation-pipeline.yaml')
-    )
-    final_filter_yaml_file_arg = DeclareLaunchArgument(
-        'final_filter_yaml_file',
-        default_value=os.path.join(
-            myPkgDir, 'params', 'final-pipeline.yaml')
+            myPkgDir, 'params', 'point-cloud-pipeline.yaml')
     )
     filter_output_layer_name_arg = DeclareLaunchArgument(
         'filter_output_layer_name',
-        default_value='output'
+        default_value='output',
+        description='The mp2p_icp metric_map_t layer name(s) to be published. Comma-separated list of more than one.'
     )
     filter_output_topic_arg = DeclareLaunchArgument(
         'filter_output_topic_name',
-        default_value='/local_map_pointcloud'
+        default_value='/local_map_pointcloud',
+        description='The topic name to publish the output layer map(s). Comma-separated list of more than one, then the number must match that of filter_output_layer_name.'
     )
     frameid_reference_arg = DeclareLaunchArgument(
         'frameid_reference',
@@ -85,10 +82,7 @@ def generate_launch_description():
             {'source_topics_pointclouds': LaunchConfiguration(
                 'points_topic_name')},
             {'show_gui': LaunchConfiguration('show_gui')},
-            {'per_obs_filter_yaml_file': LaunchConfiguration(
-                'per_obs_filter_yaml_file')},
-            {'final_filter_yaml_file': LaunchConfiguration(
-                'final_filter_yaml_file')},
+            {'pipeline_yaml_file': LaunchConfiguration('pipeline_yaml_file')},
             {'filter_output_layer_name': LaunchConfiguration(
                 'filter_output_layer_name')},
             {'time_window': LaunchConfiguration('time_window')},
@@ -110,8 +104,7 @@ def generate_launch_description():
         points_topic_name_arg,
         show_gui_arg,
         time_window_arg,
-        per_obs_filter_yaml_file_arg,
-        final_filter_yaml_file_arg,
+        pipeline_yaml_file_arg,
         filter_output_layer_name_arg,
         filter_output_topic_arg,
         frameid_reference_arg,

mrpt_pointcloud_pipeline/params/point-cloud-pipeline.yaml

@@ -0,0 +1,90 @@
+# -----------------------------------------------------------------------------
+#        mp2p_icp filters definition file for mrpt_pointcloud_pipeline
+#
+# See docs for MP2P_ICP library: https://docs.mola-slam.org/mp2p_icp/
+# -----------------------------------------------------------------------------
+
+# ---------------------------------------------------------------
+# 1) Create temporary point map to accumulate 1+ sensor observations:
+# ---------------------------------------------------------------
+generators:
+  - class_name: mp2p_icp_filters::Generator
+    params:
+      target_layer: 'accumulated_points'
+      throw_on_unhandled_observation_class: true
+      process_class_names_regex: ''  # NONE: don't process observations in the generator, just used to create the metric map.
+      #process_sensor_labels_regex: '.*'
+      # metric_map_definition_ini_file: '${CURRENT_YAML_FILE_PATH}/map_definition.ini'  # Use either an external INI file, or the 'metric_map_definition' YAML entry below
+
+      metric_map_definition:
+        # Any class derived from mrpt::maps::CMetricMap https://docs.mrpt.org/reference/latest/group_mrpt_maps_grp.html
+        class: mrpt::maps::CSimplePointsMap
+        #creationOpts:  # none needed
+        #insertionOpts: # none needed
+        #renderOpts: # none needed
+
+  # Then, use default generator: generate the observation raw points
+  - class_name: mp2p_icp_filters::Generator
+    params:
+      target_layer: 'raw'
+      throw_on_unhandled_observation_class: true
+      process_class_names_regex: '.*'
+      process_sensor_labels_regex: '.*'
+
+
+# ---------------------------------------------------------------
+# 2) Pipeline for each individual observation
+# ---------------------------------------------------------------
+per_observation:
+  # Remove the robot body:
+  - class_name: mp2p_icp_filters::FilterBoundingBox
+    params:
+      input_pointcloud_layer: 'raw'
+      outside_pointcloud_layer: 'filtered'
+      bounding_box_min: [ -1.0, -1.0, -2 ]
+      bounding_box_max: [  1.0,  1.0,  2 ]
+
+  - class_name: mp2p_icp_filters::FilterMerge
+    params:
+      input_pointcloud_layer: 'filtered'
+      target_layer: 'accumulated_points'
+
+# ---------------------------------------------------------------
+# 3) Pipeline to apply to the merged data
+# ---------------------------------------------------------------
+final:
+  # Remove points that are too far:
+  - class_name: mp2p_icp_filters::FilterBoundingBox
+    params:
+      input_pointcloud_layer: 'accumulated_points'
+      inside_pointcloud_layer: 'layer1'
+      bounding_box_min: [ -20.0, -20.0,  0.1 ]
+      bounding_box_max: [  20.0,  20.0,  1.5 ]
+
+  # Split into nearby and distant
+  - class_name: mp2p_icp_filters::FilterBoundingBox
+    params:
+      input_pointcloud_layer: 'layer1'
+      inside_pointcloud_layer: 'close'
+      outside_pointcloud_layer: 'far'
+      bounding_box_min: [ -6.0, -6.0, -3.0 ]
+      bounding_box_max: [  6.0,  6.0,  3.0 ]
+
+  # Downsample points:
+  - class_name: mp2p_icp_filters::FilterDecimateVoxels
+    params:
+      input_pointcloud_layer: 'close'
+      output_pointcloud_layer: 'output'
+      voxel_filter_resolution: 0.10  # [m]
+      decimate_method: DecimateMethod::ClosestToAverage
+      # This option flattens the 3D point cloud into a 2D one:
+      flatten_to: 1.0 # [m]
+
+  - class_name: mp2p_icp_filters::FilterDecimateVoxels
+    params:
+      input_pointcloud_layer: 'far'
+      output_pointcloud_layer: 'output'
+      voxel_filter_resolution: 0.5  # [m]
+      decimate_method: DecimateMethod::ClosestToAverage
+      # This option flattens the 3D point cloud into a 2D one:
+      flatten_to: 1.0 # [m]