From d639872eab8f6c2e918c845d5917d8f95dfb6394 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 7 Feb 2024 12:43:36 +0100 Subject: [PATCH] Use just one pipeline generator+filter definition file --- .../src/mrpt_pf_localization_node.cpp | 29 ++- .../mrpt_pointcloud_pipeline_node.h | 20 +- .../launch/pointcloud_pipeline.launch.py | 25 +-- .../params/final-pipeline.yaml | 41 ---- .../params/per-observation-pipeline.yaml | 13 -- .../params/point-cloud-pipeline.yaml | 90 +++++++++ .../src/mrpt_pointcloud_pipeline_node.cpp | 177 +++++++++--------- 7 files changed, 220 insertions(+), 175 deletions(-) delete mode 100644 mrpt_pointcloud_pipeline/params/final-pipeline.yaml delete mode 100644 mrpt_pointcloud_pipeline/params/per-observation-pipeline.yaml create mode 100644 mrpt_pointcloud_pipeline/params/point-cloud-pipeline.yaml diff --git a/mrpt_pf_localization/src/mrpt_pf_localization_node.cpp b/mrpt_pf_localization/src/mrpt_pf_localization_node.cpp index 6b23aee..13d48ad 100644 --- a/mrpt_pf_localization/src/mrpt_pf_localization_node.cpp +++ b/mrpt_pf_localization/src/mrpt_pf_localization_node.cpp @@ -22,12 +22,27 @@ #include #include #include +#include #include #include #include #include +#if MRPT_VERSION >= 0x020b08 +#include +#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( 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); diff --git a/mrpt_pointcloud_pipeline/include/mrpt_pointcloud_pipeline/mrpt_pointcloud_pipeline_node.h b/mrpt_pointcloud_pipeline/include/mrpt_pointcloud_pipeline/mrpt_pointcloud_pipeline_node.h index b846bb1..d117e0a 100644 --- a/mrpt_pointcloud_pipeline/include/mrpt_pointcloud_pipeline/mrpt_pointcloud_pipeline_node.h +++ b/mrpt_pointcloud_pipeline/include/mrpt_pointcloud_pipeline/mrpt_pointcloud_pipeline_node.h @@ -9,6 +9,7 @@ /* mrpt deps*/ #include #include +#include #include #include #include @@ -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::SharedPtr pub; + }; + std::vector layer2topic_; /** * @name ROS2 pubs/subs * @{ */ - rclcpp::Publisher::SharedPtr - m_pub_local_map_pointcloud; std::vector::SharedPtr> m_subs_2dlaser; std::vector::SharedPtr> diff --git a/mrpt_pointcloud_pipeline/launch/pointcloud_pipeline.launch.py b/mrpt_pointcloud_pipeline/launch/pointcloud_pipeline.launch.py index fdcc21b..a559b51 100644 --- a/mrpt_pointcloud_pipeline/launch/pointcloud_pipeline.launch.py +++ b/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, diff --git a/mrpt_pointcloud_pipeline/params/final-pipeline.yaml b/mrpt_pointcloud_pipeline/params/final-pipeline.yaml deleted file mode 100644 index 58bf3db..0000000 --- a/mrpt_pointcloud_pipeline/params/final-pipeline.yaml +++ /dev/null @@ -1,41 +0,0 @@ -# ----------------------------------------------------------------------------- -# mp2p_icp filters definition file -# -# See docs for MP2P_ICP library: https://docs.mola-slam.org/mp2p_icp/ -# ----------------------------------------------------------------------------- - -# Remove points that are too far: -- class_name: mp2p_icp_filters::FilterBoundingBox - params: - input_pointcloud_layer: 'raw' - 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] - #use_voxel_average: true - # 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] - #use_voxel_average: true - # This option flattens the 3D point cloud into a 2D one: - flatten_to: 1.0 # [m] diff --git a/mrpt_pointcloud_pipeline/params/per-observation-pipeline.yaml b/mrpt_pointcloud_pipeline/params/per-observation-pipeline.yaml deleted file mode 100644 index c952cde..0000000 --- a/mrpt_pointcloud_pipeline/params/per-observation-pipeline.yaml +++ /dev/null @@ -1,13 +0,0 @@ -# ----------------------------------------------------------------------------- -# mp2p_icp filters definition file -# -# See docs for MP2P_ICP library: https://docs.mola-slam.org/mp2p_icp/ -# ----------------------------------------------------------------------------- - -# Remove the robot body: -- class_name: mp2p_icp_filters::FilterBoundingBox - params: - input_pointcloud_layer: 'raw' - outside_pointcloud_layer: 'output' - bounding_box_min: [ -1.0, -1.0, -2 ] - bounding_box_max: [ 1.0, 1.0, 2 ] diff --git a/mrpt_pointcloud_pipeline/params/point-cloud-pipeline.yaml b/mrpt_pointcloud_pipeline/params/point-cloud-pipeline.yaml new file mode 100644 index 0000000..ebf2718 --- /dev/null +++ b/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] diff --git a/mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_node.cpp b/mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_node.cpp index 2054dbf..aeb3974 100644 --- a/mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_node.cpp +++ b/mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_node.cpp @@ -27,11 +27,6 @@ LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options) read_parameters(); - // Create publisher for local map point cloud - m_pub_local_map_pointcloud = - this->create_publisher( - m_topic_local_map_pointcloud, 10); - // Init ROS subs: // Subscribe to one or more laser sources: size_t nSubsTotal = 0; @@ -65,10 +60,6 @@ LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options) rclcpp::shutdown(); } - // Local map params: - m_localmap_pts->insertionOptions.minDistBetweenLaserPoints = 0; - m_localmap_pts->insertionOptions.also_interpolate = false; - // Create the tf2 buffer and listener m_tf_buffer = std::make_shared(this->get_clock()); m_tf_listener = std::make_shared(*m_tf_buffer); @@ -137,7 +128,8 @@ void LocalObstaclesNode::on_do_publish() // Build local map(s): // ----------------------------------------------- - m_localmap_pts->clear(); + mp2p_icp::metric_map_t mm; + mrpt::poses::CPose3D curRobotPose; { CTimeLoggerEntry tle2(m_profiler, "on_do_publish.buildLocalMap"); @@ -176,54 +168,33 @@ void LocalObstaclesNode::on_do_publish() const mrpt::poses::CPose3D relPose = ipt.robot_pose - curRobotPose; // Insert obs: - if (m_per_obs_pipeline.empty()) - { - // direct insertion w/o filtering: - m_localmap_pts->insertObservationPtr(ipt.observation, relPose); - } - else - { - CTimeLoggerEntry tleObsFilter( - m_profiler, "on_do_publish.apply_per_obs_pipeline"); + CTimeLoggerEntry tleObsFilter( + m_profiler, "on_do_publish.apply_per_obs_pipeline"); - // per-observation filtering: - auto auxPts = CSimplePointsMap::Create(); - auxPts->insertObservationPtr(ipt.observation); + // Apply optional generators for auxiliary map layers, etc: + mp2p_icp_filters::apply_generators( + m_generator, *ipt.observation, mm); - mp2p_icp::metric_map_t mm; - mm.layers[mp2p_icp::metric_map_t::PT_LAYER_RAW] = auxPts; - mp2p_icp_filters::apply_filter_pipeline(m_per_obs_pipeline, mm); + // per-observation filtering: + mp2p_icp_filters::apply_filter_pipeline(m_per_obs_pipeline, mm); - const mrpt::maps::CPointsMap::Ptr outPts = - mm.point_layer(m_filter_output_layer_name); - m_localmap_pts->insertAnotherMap(outPts.get(), relPose); - } + tleObsFilter.stop(); } } - // Filtering: - mrpt::maps::CPointsMap::Ptr filteredPts; - - if (!m_final_pipeline.empty()) - { - // Apply final filtering: - CTimeLoggerEntry tleFilter( - m_profiler, "on_do_publish.apply_final_pipeline"); + // Apply final filtering: + CTimeLoggerEntry tleFilter( + m_profiler, "on_do_publish.apply_final_pipeline"); - mp2p_icp::metric_map_t mm; - mm.layers[mp2p_icp::metric_map_t::PT_LAYER_RAW] = m_localmap_pts; - mp2p_icp_filters::apply_filter_pipeline(m_final_pipeline, mm); + mp2p_icp_filters::apply_filter_pipeline(m_final_pipeline, mm); - filteredPts = mm.point_layer(m_filter_output_layer_name); - } - else - { - filteredPts = m_localmap_pts; - } + tleFilter.stop(); // Publish them: - if (m_pub_local_map_pointcloud->get_subscription_count() > 0) + for (auto& e : layer2topic_) { + if (e.pub->get_subscription_count() == 0) continue; + sensor_msgs::msg::PointCloud2 msg_pts; msg_pts.header.frame_id = m_frameid_robot; @@ -231,15 +202,18 @@ void LocalObstaclesNode::on_do_publish() msg_pts.header.stamp = mrpt::ros2bridge::toROS( mrpt::Clock::fromDouble(obs.rbegin()->first)); + const auto& outPtsMap = mm.point_layer(e.layer); + ASSERT_(outPtsMap); + // TODO(jlbc): We could add an if-else chain if XYZI / XYZIRT point // clouds are desired as output too, but it seems not likely for this // kind of filtering node. - auto simplPts = std::dynamic_pointer_cast( - filteredPts); + auto simplPts = + std::dynamic_pointer_cast(outPtsMap); ASSERT_(simplPts); mrpt::ros2bridge::toROS(*simplPts, msg_pts.header, msg_pts); - m_pub_local_map_pointcloud->publish(msg_pts); + e.pub->publish(msg_pts); } // Show gui: @@ -331,11 +305,22 @@ void LocalObstaclesNode::on_do_publish() gl_obs->insert(gl_axis); } // end for - glRawPts->loadFromPointsMap(m_localmap_pts.get()); - glRawPts->setVisibility(m_visible_raw); + if (auto pts = mm.point_layer(mp2p_icp::metric_map_t::PT_LAYER_RAW); + pts) + { + glRawPts->loadFromPointsMap(pts.get()); + glRawPts->setVisibility(m_visible_raw); + } - glFinalPts->loadFromPointsMap(filteredPts.get()); - glFinalPts->setVisibility(m_visible_output); + // This shows the first output layer only (!). TODO: show all of them? + if (!layer2topic_.empty()) + { + if (auto pts = mm.point_layer(layer2topic_.front().layer); pts) + { + glFinalPts->loadFromPointsMap(pts.get()); + glFinalPts->setVisibility(m_visible_output); + } + } m_gui_win->unlockAccess3DScene(); m_gui_win->repaint(); @@ -562,14 +547,6 @@ void LocalObstaclesNode::read_parameters() // publish_period can't be larger than m_time_window: ASSERT_LE_(m_publish_period, m_time_window); - this->declare_parameter( - "topic_local_map_pointcloud", "local_map_pointcloud"); - this->get_parameter( - "topic_local_map_pointcloud", m_topic_local_map_pointcloud); - RCLCPP_INFO( - get_logger(), "topic_local_map_pointcloud: %s", - m_topic_local_map_pointcloud.c_str()); - this->declare_parameter( "source_topics_2d_scans", "scan, laser1"); this->get_parameter("source_topics_2d_scans", m_topics_source_2dscan); @@ -584,44 +561,64 @@ void LocalObstaclesNode::read_parameters() get_logger(), "source_topics_pointclouds: %s", m_topics_source_pointclouds.c_str()); - this->declare_parameter("per_obs_filter_yaml_file", ""); - this->get_parameter("per_obs_filter_yaml_file", m_per_obs_filter_yaml_file); + this->declare_parameter( + "pipeline_yaml_file", m_pipeline_yaml_file); + this->get_parameter("pipeline_yaml_file", m_pipeline_yaml_file); RCLCPP_INFO( - get_logger(), "per_obs_filter_yaml_file: %s", - m_per_obs_filter_yaml_file.c_str()); - if (!m_per_obs_filter_yaml_file.empty()) + get_logger(), "pipeline_yaml_file: %s", m_pipeline_yaml_file.c_str()); { - ASSERT_FILE_EXISTS_(m_per_obs_filter_yaml_file); + ASSERT_FILE_EXISTS_(m_pipeline_yaml_file); const mrpt::containers::yaml cfg = - mrpt::containers::yaml::FromFile(m_per_obs_filter_yaml_file); - std::stringstream ss; - cfg.printAsYAML(ss); - RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); - m_per_obs_pipeline = mp2p_icp_filters::filter_pipeline_from_yaml(cfg); + mrpt::containers::yaml::FromFile(m_pipeline_yaml_file); + + RCLCPP_DEBUG_STREAM(get_logger(), cfg); + + ASSERT_(cfg.has("generators")); + ASSERT_(cfg.has("per_observation")); + ASSERT_(cfg.has("final")); + + m_generator = mp2p_icp_filters::generators_from_yaml(cfg["generators"]); + m_per_obs_pipeline = + mp2p_icp_filters::filter_pipeline_from_yaml(cfg["per_observation"]); + m_final_pipeline = + mp2p_icp_filters::filter_pipeline_from_yaml(cfg["final"]); } - this->declare_parameter("final_filter_yaml_file", ""); - this->get_parameter("final_filter_yaml_file", m_final_filter_yaml_file); + // Output layer(s) ==> ROS topic(s) mapping: + // -------------------------------------------------- + std::string filter_output_layer_name = "output"; + std::string topic_local_map_pointcloud = "local_map_pointcloud"; + + this->declare_parameter( + "topic_local_map_pointcloud", topic_local_map_pointcloud); + this->get_parameter( + "topic_local_map_pointcloud", topic_local_map_pointcloud); RCLCPP_INFO( - get_logger(), "final_filter_yaml_file: %s", - m_final_filter_yaml_file.c_str()); - if (!m_final_filter_yaml_file.empty()) - { - ASSERT_FILE_EXISTS_(m_final_filter_yaml_file); - const mrpt::containers::yaml cfg = - mrpt::containers::yaml::FromFile(m_final_filter_yaml_file); - std::stringstream ss; - cfg.printAsYAML(ss); - RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); - m_final_pipeline = mp2p_icp_filters::filter_pipeline_from_yaml(cfg); - } + get_logger(), "topic_local_map_pointcloud: %s", + topic_local_map_pointcloud.c_str()); this->declare_parameter( - "filter_output_layer_name", m_filter_output_layer_name); - this->get_parameter("filter_output_layer_name", m_filter_output_layer_name); + "filter_output_layer_name", filter_output_layer_name); + this->get_parameter("filter_output_layer_name", filter_output_layer_name); RCLCPP_INFO( get_logger(), "filter_output_layer_name: %s", - m_filter_output_layer_name.c_str()); + filter_output_layer_name.c_str()); + + // parse lists: + std::vector lstLayers, lstTopics; + mrpt::system::tokenize(filter_output_layer_name, ", \t\r\n", lstLayers); + mrpt::system::tokenize(topic_local_map_pointcloud, ", \t\r\n", lstTopics); + ASSERT_EQUAL_(lstLayers.size(), lstTopics.size()); + for (size_t i = 0; i < lstLayers.size(); i++) + { + auto& e = layer2topic_.emplace_back(); + e.layer = lstLayers.at(i); + e.topic = lstTopics.at(i); + + // Create publisher for local map point cloud: + e.pub = + this->create_publisher(e.topic, 10); + } } int main(int argc, char** argv)