diff --git a/CMakeLists.txt b/CMakeLists.txt index 02e2e3e..2f52a73 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,10 +11,12 @@ find_package(geometry_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) -find_package(libpointmatcher_ros REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(libpointmatcher CONFIG 1.4.2 REQUIRED) -find_package(norlab_icp_mapper CONFIG 2.0.0 REQUIRED) +find_package(norlab_icp_mapper CONFIG 2.1.0 REQUIRED) +find_package(libpointmatcher_ros 2.0.1 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(OpenMP REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/SaveTrajectory.srv" @@ -27,7 +29,11 @@ include_directories( ${norlab_icp_mapper_INCLUDE_DIRS} ) -add_executable(mapper_node src/mapper_node.cpp src/NodeParameters.cpp) +add_executable(mapper_node + src/mapper_node.cpp + src/NodeParameters.cpp + src/Deskewer.cpp +) ament_target_dependencies(mapper_node rclcpp @@ -38,9 +44,14 @@ ament_target_dependencies(mapper_node tf2_ros tf2 libpointmatcher_ros + tf2_geometry_msgs ) + +# Add OpenMP flags and link OpenMP library +target_compile_options(mapper_node PRIVATE ${OpenMP_CXX_FLAGS}) target_link_libraries(mapper_node ${norlab_icp_mapper_LIBRARIES} + OpenMP::OpenMP_CXX ) rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") @@ -51,5 +62,10 @@ install(TARGETS mapper_node DESTINATION lib/${PROJECT_NAME} ) -ament_package() +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/README.md b/README.md index fcbe6bd..4adbb75 100644 --- a/README.md +++ b/README.md @@ -1,37 +1,45 @@ # norlab_icp_mapper_ros + A bridge between [norlab_icp_mapper](https://github.com/norlab-ulaval/norlab_icp_mapper/) and ROS. Check the [mapper's documentation](https://norlab-icp-mapper.readthedocs.io/en/latest/UsingInRos/) for a detailed guide. ## Node Parameters -| Name | Description | Possible values | Default Value | -|:---------------------------------:|:-----------------------------------------------------------------------------------------------------------:|:------------------------------:|:----------------------------------------------------------:| -| odom_frame | Frame used for odometry. | Any string | "odom" | -| robot_frame | Frame centered on the robot. | Any string | "base_link" | -| mapping_config | Path to the file containing the mapping config. | Any file path | "" | -| initial_map_file_name | Path of the file from which the initial map is loaded. | Any file path | "" | -| initial_robot_pose | Transformation matrix in homogeneous coordinates describing the initial pose of the robot in the map frame. | Any matrix of dimension 3 or 4 | "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]" | -| final_map_file_name | Path of the file in which the final map is saved when is_online is false. | Any file path | "map.vtk" | -| final_trajectory_file_name | Path of the file in which the final trajectory is saved when is_online is false. | Any file path | "trajectory.vtk" | -| map_publish_rate | Rate at which the map is published (in Hertz). It can be slower depending on the map update rate. | (0, ∞) | 10.0 | -| map_tf_publish_rate | Rate at which the map tf is published (in Hertz). | (0, ∞) | 10.0 | -| max_idle_time | Delay to wait being idle before shutting down ROS when is_online is false (in seconds). | [0, ∞) | 10.0 | -| is_mapping | true when map updates are wanted, false when only localization is wanted. | {true, false} | true | -| is_online | true when online mapping is wanted, false otherwise. | {true, false} | true | -| is_3D | true when a 3D sensor is used, false when a 2D sensor is used. | {true, false} | true | -| save_map_cells_on_hard_drive | true when map cell storage on hard drive is wanted, false when map cell storage in RAM is wanted. | {true, false} | true | -| publish_tfs_between_registrations | When false, the map tf is published only after registration. Otherwise with map_tf_publish_rate. | {true, false} | true | +| Name | Description | Possible values | Default Value | +| :---------------------------------: | :---------------------------------------------------------------------------------------------------------: | :----------------------------: | :--------------------------------------------------------: | +| odom_frame | Frame used for odometry. | Any string | "odom" | +| robot_frame | Frame centered on the robot. | Any string | "base_link" | +| mapping_config | Path to the file containing the mapping config. | Any file path | "" | +| initial_map_file_name | Path of the file from which the initial map is loaded. | Any file path | "" | +| initial_robot_pose | Transformation matrix in homogeneous coordinates describing the initial pose of the robot in the map frame. | Any matrix of dimension 3 or 4 | "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]" | +| final_map_file_name | Path of the file in which the final map is saved when is_online is false. | Any file path | "map.vtk" | +| final_trajectory_file_name | Path of the file in which the final trajectory is saved when is_online is false. | Any file path | "trajectory.vtk" | +| map_publish_rate | Rate at which the map is published (in Hertz). It can be slower depending on the map update rate. | (0, ∞) | 10.0 | +| map_tf_publish_rate | Rate at which the map tf is published (in Hertz). | (0, ∞) | 10.0 | +| max_idle_time | Delay to wait being idle before shutting down ROS when is_online is false (in seconds). | [0, ∞) | 10.0 | +| is_mapping | true when map updates are wanted, false when only localization is wanted. | {true, false} | true | +| is_online | true when online mapping is wanted, false otherwise. | {true, false} | true | +| is_3D | true when a 3D sensor is used, false when a 2D sensor is used. | {true, false} | true | +| save_map_cells_on_hard_drive | true when map cell storage on hard drive is wanted, false when map cell storage in RAM is wanted. | {true, false} | true | +| publish_tfs_between_registrations | When false, the map tf is published only after registration. Otherwise with map_tf_publish_rate. | {true, false} | true | +| deskew | Set to true if you want to apply motion compensation on the input point cloud. | {true, false} | true | +| expected_unique_deskewing_TF_number | How much memory should we reserve for the per-point transformations. | A positive integer | 4000 | +| deskewing_round_to_nanosecs | How much should each point's timestamp be rounded for deskewing transformation search. | A positive integer | 50000 | ## Node Topics -| Name | Description | -|:---------:|:---------------------------------------------------:| -| points_in | Topic from which the input points are retrieved. | -| map | Topic in which the map is published. | -| icp_odom | Topic in which the corrected odometry is published. | + +| Name | Description | +| :----------------------: | :----------------------------------------------------------------: | +| points_in | Topic from which the input points are retrieved. | +| map | Topic in which the map is published. | +| icp_odom | Topic in which the corrected odometry is published. | +| scan_after_input_filters | The input scan, after all input filters are applied. | +| scan_after_deskew | The input scan, after all input filters and deskewing are applied. | ## Node Services + | Name | Description | Parameter Name | Parameter Description | -|:------------------:|:-----------------------------:|:--------------:|:--------------------------------------------------:| +| :----------------: | :---------------------------: | :------------: | :------------------------------------------------: | | save_map | Saves the current map. | filename | Path of the file in which the map is saved. | | save_trajectory | Saves the current trajectory. | filename | Path of the file in which the trajectory is saved. | | reload_yaml_config | Reload the YAML config file. | | | @@ -46,6 +54,8 @@ flowchart LR /points_in([ /points_in
sensor_msgs/msg/PointCloud2 ]):::bugged /icp_odom([ /icp_odom
nav_msgs/msg/Odometry ]):::bugged /map([ /map
sensor_msgs/msg/PointCloud2 ]):::bugged +/scan_after_input_filters([ /map
sensor_msgs/msg/PointCloud2 ]):::bugged +/scan_after_deskew([ /map
sensor_msgs/msg/PointCloud2 ]):::bugged /disable_mapping[/ /disable_mapping
std_srvs/srv/Empty \]:::bugged /enable_mapping[/ /enable_mapping
std_srvs/srv/Empty \]:::bugged /load_map[/ /load_map
norlab_icp_mapper_ros/srv/LoadMap \]:::bugged @@ -92,4 +102,3 @@ style nodes opacity:0.15,fill:#FFF style connection opacity:0.15,fill:#FFF ``` - diff --git a/config/mapper.yaml b/config/mapper.yaml new file mode 100644 index 0000000..c691fe8 --- /dev/null +++ b/config/mapper.yaml @@ -0,0 +1,66 @@ +mapper: + updateCondition: + type: delay + value: 0.5 + + sensorMaxRange: 100 + + mapperModule: + - PointDistanceMapperModule: + minDistNewPoint: 0.1 + +input: + - RandomSamplingDataPointsFilter: + prob: 0.8 + + - AddDescriptorDataPointsFilter: + descriptorName: probabilityDynamic + descriptorDimension: 1 + descriptorValues: [0.6] + + - SamplingSurfaceNormalDataPointsFilter: + ratio: 0.5 + knn: 7 + samplingMethod: 0 + maxBoxDim: inf + averageExistingDescriptors: 1 + keepNormals: 1 + keepDensities: 0 + keepEigenValues: 0 + keepEigenVectors: 0 + +icp: + readingDataPointsFilters: + - RandomSamplingDataPointsFilter: + prob: 0.8 + + referenceDataPointsFilters: + + outlierFilters: + - TrimmedDistOutlierFilter: + ratio: 0.75 + + matcher: + KDTreeMatcher: + knn: 3 + epsilon: 1 + searchType: 1 + maxDist: 200.0 + + errorMinimizer: + PointToPlaneErrorMinimizer: + force4DOF: 1 + + transformationCheckers: + - CounterTransformationChecker: + maxIterationCount: 40 + - DifferentialTransformationChecker: + minDiffRotErr: 0.001 + minDiffTransErr: 0.001 + smoothLength: 3 + + inspector: NullInspector + + logger: NullLogger + +post: diff --git a/launch/mapper.launch.py b/launch/mapper.launch.py new file mode 100644 index 0000000..bb453f9 --- /dev/null +++ b/launch/mapper.launch.py @@ -0,0 +1,48 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation time" + ), + Node( + package="norlab_icp_mapper_ros", + executable="mapper_node", + name="mapper_node", + output="screen", + parameters=[ + { + "use_sim_time": True, + "odom_frame": "odom", + "robot_frame": "base_link", + "mapping_config": os.path.join( + get_package_share_directory("norlab_icp_mapper_ros"), + "config", + "mapper.yaml", + ), + "initial_map_file_name": "", + "initial_robot_pose": "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]", + "final_map_file_name": "map.vtk", + "final_trajectory_file_name": "trajectory.vtk", + "map_publish_rate": 10.0, + "map_tf_publish_rate": 10.0, + "max_idle_time": 10.0, + "is_mapping": True, + "is_online": True, + "is_3D": True, + "save_map_cells_on_hard_drive": True, + "publish_tfs_between_registrations": True, + } + ], + remappings=[ + ("points_in", "lslidar_point_cloud"), + ], + ), + ] + ) diff --git a/package.xml b/package.xml index 36352b6..759c7e1 100644 --- a/package.xml +++ b/package.xml @@ -2,13 +2,14 @@ norlab_icp_mapper_ros - 2.0.0 + 2.1.0 A bridge between norlab_icp_mapper and ROS. Simon-Pierre Deschenes + Matej Boxan new BSD - + ament_cmake - + rclcpp std_msgs sensor_msgs @@ -16,6 +17,7 @@ std_srvs tf2_ros tf2 + tf2_geometry_msgs libpointmatcher_ros rosidl_default_generators @@ -23,7 +25,7 @@ rosidl_default_runtime rosidl_interface_packages - + ament_cmake diff --git a/src/Deskewer.cpp b/src/Deskewer.cpp new file mode 100644 index 0000000..919d708 --- /dev/null +++ b/src/Deskewer.cpp @@ -0,0 +1,77 @@ +#include "Deskewer.h" +#include +#include +#include + +#include +#include +#include +#include + +Deskewer::Deskewer(const rclcpp::Logger& logger, rclcpp::Clock::SharedPtr clock, uint expectedUniqueDeskewingTFNumber, uint deskewingRoundToNanoSecs) + : logger(logger), + expectedUniqueDeskewingTFNumber(expectedUniqueDeskewingTFNumber), + deskewingRoundToNanoSecs(deskewingRoundToNanoSecs) +{ + tfBuffer = std::unique_ptr(new tf2_ros::Buffer(clock)); + tfListener = std::unique_ptr(new tf2_ros::TransformListener(*tfBuffer)); + tfsCache.reserve(expectedUniqueDeskewingTFNumber); +} + +bool Deskewer::deskewCloud(Deskewer::DP &cloud, const std::string &sensorFrame) +{ + std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); + + tfsCache.clear(); + + if (!cloud.timeExists(timeFieldName)) + { + RCLCPP_WARN(logger, "The input pointcloud does not contain the 'time' field. Skipping."); + return false; + } + + int64_t latestTime = 0; + for (int i=0; ilookupTransform(sensorFrame, + latestTimeRos, + sensorFrame, + laserTimeRos, + fixedFrameForLaser, + rclcpp::Duration(0, 2.5e8)); + tfsCache[cachedTfTime] = transform; + } + catch(tf2::TransformException &ex){ + RCLCPP_ERROR(logger, "Pointcloud callback failed because: %s", ex.what()); + return false; + } + } + } + + // apply the transforms to the pointcloud in parallel + #pragma omp parallel for + for (int i=0; i(transform, 4); + cloud.features.col(i) = transformationParameters * cloud.features.col(i); + } + + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + RCLCPP_DEBUG_STREAM(logger, "Point cloud deskewed in " << std::chrono::duration_cast(end - begin).count() << " [ms]"); + return true; +} diff --git a/src/Deskewer.h b/src/Deskewer.h new file mode 100644 index 0000000..5d1dfd8 --- /dev/null +++ b/src/Deskewer.h @@ -0,0 +1,36 @@ +#ifndef DESKEWER_H +#define DESKEWER_H + +#include +#include +#include +#include +#include +#include + +class Deskewer +{ + private: + typedef PointMatcher PM; + typedef PM::DataPoints DP; + + uint expectedUniqueDeskewingTFNumber; + uint deskewingRoundToNanoSecs; + std::unique_ptr tfBuffer = nullptr; + std::unique_ptr tfListener = nullptr; + rclcpp::Logger logger; + + const std::string timeFieldName = "time"; + std::string fixedFrameForLaser = "odom"; + + // Dictionary to store already looked up transforms + std::unordered_map tfsCache; + + public: + // Constructor + Deskewer(const rclcpp::Logger &logger, rclcpp::Clock::SharedPtr clock, uint expectedUniqueDeskewingTFNumber, uint deskewingRoundToNanoSecs); + + bool deskewCloud(DP &cloud, const std::string &sensor_frame); +}; + +#endif diff --git a/src/NodeParameters.cpp b/src/NodeParameters.cpp index a95e256..0415a0f 100644 --- a/src/NodeParameters.cpp +++ b/src/NodeParameters.cpp @@ -26,6 +26,9 @@ void NodeParameters::declareParameters(rclcpp::Node& node) node.declare_parameter("is_online", true); node.declare_parameter("save_map_cells_on_hard_drive", true); node.declare_parameter("publish_tfs_between_registrations", true); + node.declare_parameter("deskew", true); + node.declare_parameter("expected_unique_deskewing_TF_number", 4000); + node.declare_parameter("deskewing_round_to_nanosecs", 50000); } void NodeParameters::retrieveParameters(rclcpp::Node& node) @@ -45,6 +48,9 @@ void NodeParameters::retrieveParameters(rclcpp::Node& node) node.get_parameter("is_online", isOnline); node.get_parameter("save_map_cells_on_hard_drive", saveMapCellsOnHardDrive); node.get_parameter("publish_tfs_between_registrations", publishTfsBetweenRegistrations); + node.get_parameter("deskew", deskew); + node.get_parameter("expected_unique_deskewing_TF_number", expectedUniqueDeskewingTFNumber); + node.get_parameter("deskewing_round_to_nanosecs", deskewingRoundToNanoSecs); } void NodeParameters::validateParameters() const @@ -110,6 +116,18 @@ void NodeParameters::validateParameters() const { throw std::runtime_error("is mapping is set to false, but initial map file name was not specified."); } + + if (deskew) + { + if (expectedUniqueDeskewingTFNumber <= 0) + { + throw std::runtime_error("Expected unique deskewing TF value must be positive: " + std::to_string(expectedUniqueDeskewingTFNumber)); + } + if (deskewingRoundToNanoSecs <= 0) + { + throw std::runtime_error("Deskewing round to nsecs value must be positive: " + std::to_string(deskewingRoundToNanoSecs)); + } + } } void NodeParameters::parseComplexParameters() diff --git a/src/NodeParameters.h b/src/NodeParameters.h index 745b2ef..7a7cb70 100644 --- a/src/NodeParameters.h +++ b/src/NodeParameters.h @@ -32,6 +32,9 @@ class NodeParameters bool isOnline; bool saveMapCellsOnHardDrive; bool publishTfsBetweenRegistrations; + bool deskew; + int expectedUniqueDeskewingTFNumber; + int deskewingRoundToNanoSecs; NodeParameters(rclcpp::Node& node); }; diff --git a/src/mapper_node.cpp b/src/mapper_node.cpp index c6543f9..314a166 100644 --- a/src/mapper_node.cpp +++ b/src/mapper_node.cpp @@ -1,12 +1,13 @@ +#include "Deskewer.h" #include "NodeParameters.h" +#include +#include #include #include #include #include #include #include -#include -#include #include #include #include @@ -26,6 +27,8 @@ class MapperNode : public rclcpp::Node mapper = std::make_unique(params->mappingConfig, params->is3D, params->isOnline, params->isMapping, params->saveMapCellsOnHardDrive); + deskewer = std::make_unique(this->get_logger(), this->get_clock(), params->expectedUniqueDeskewingTFNumber, params->deskewingRoundToNanoSecs); + if(!params->initialMapFileName.empty()) { loadMap(params->initialMapFileName); @@ -52,6 +55,8 @@ class MapperNode : public rclcpp::Node tfBroadcaster = std::unique_ptr(new tf2_ros::TransformBroadcaster(*this)); mapPublisher = this->create_publisher("map", 2); + inputFiltersScanPublisher = this->create_publisher("scan_after_input_filters", 1); + deskewingScanPublisher = this->create_publisher("scan_after_deskew", 1); odomPublisher = this->create_publisher("icp_odom", 50); if(params->is3D) @@ -115,6 +120,8 @@ class MapperNode : public rclcpp::Node std::mutex mapTfLock; PM::TransformationParameters odomToMap; rclcpp::Publisher::SharedPtr mapPublisher; + rclcpp::Publisher::SharedPtr inputFiltersScanPublisher; + rclcpp::Publisher::SharedPtr deskewingScanPublisher; rclcpp::Publisher::SharedPtr odomPublisher; rclcpp::Subscription::SharedPtr pointCloud2Subscription; rclcpp::Subscription::SharedPtr laserScanSubscription; @@ -129,6 +136,8 @@ class MapperNode : public rclcpp::Node std::thread mapPublisherThread; std::thread mapTfPublisherThread; + std::unique_ptr deskewer; + std::string appendToFilePath(const std::string& filePath, const std::string& suffix) { std::string::size_type const extensionPosition(filePath.find_last_of('.')); @@ -199,10 +208,30 @@ class MapperNode : public rclcpp::Node return PointMatcher_ROS::rosTfToPointMatcherTransformation(tf, transformDimension); } - void gotInput(const PM::DataPoints& input, const std::string& sensorFrame, const rclcpp::Time& timeStamp) + void gotInput(PM::DataPoints& input, const std::string& sensorFrame, const rclcpp::Time& cloudStamp) { + rclcpp::Time timeStamp = cloudStamp; + std::chrono::steady_clock::time_point processingStartTime = std::chrono::steady_clock::now(); try { + mapper->applyInputFilters(input); + std::chrono::steady_clock::time_point filterEndTime = std::chrono::steady_clock::now(); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Applied input filters in " << std::chrono::duration_cast(filterEndTime - processingStartTime).count() << " [ms]"); + publishAfterInputFilters(input, sensorFrame, cloudStamp); + + if (params->deskew) + { + bool deskewSuccessuful = deskewer->deskewCloud(input, sensorFrame); + + // if deskewing was successful, update the cloud timestamp to match the last point in the cloud + if (deskewSuccessuful) + { + publishAfterDeskew(input, sensorFrame, cloudStamp); + timeStamp = rclcpp::Time(input.times(input.getNbPoints() - 1), timeStamp.get_clock_type()); + } + } + + PM::TransformationParameters sensorToOdom = findTransform(sensorFrame, params->odomFrame, timeStamp, input.getHomogeneousDim()); PM::TransformationParameters sensorToMapBeforeUpdate = odomToMap * sensorToOdom; @@ -214,8 +243,11 @@ class MapperNode : public rclcpp::Node } try { + std::chrono::steady_clock::time_point mappingStartTime = std::chrono::steady_clock::now(); mapper->processInput(input, sensorToMapBeforeUpdate, std::chrono::time_point(std::chrono::nanoseconds(timeStamp.nanoseconds()))); + std::chrono::steady_clock::time_point mappingEndTime = std::chrono::steady_clock::now(); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Mapper call executed in: " << std::chrono::duration_cast(mappingEndTime - mappingStartTime).count() << " [ms]"); } catch (const PM::ConvergenceError& convergenceError) { @@ -272,16 +304,48 @@ class MapperNode : public rclcpp::Node { RCLCPP_WARN(this->get_logger(), "%s", ex.what()); } + + std::chrono::steady_clock::time_point processingEndTime = std::chrono::steady_clock::now(); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Mapping finished in " << std::chrono::duration_cast(processingEndTime - processingStartTime).count() << " [ms]"); + } void pointCloud2Callback(const sensor_msgs::msg::PointCloud2& cloudMsgIn) { - gotInput(PointMatcher_ROS::rosMsgToPointMatcherCloud(cloudMsgIn), cloudMsgIn.header.frame_id, cloudMsgIn.header.stamp); + RCLCPP_DEBUG(this->get_logger(), "----POINT CLOUD RECEIVED----"); + std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); + auto input = PointMatcher_ROS::rosMsgToPointMatcherCloud(cloudMsgIn); + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Input converted in " << std::chrono::duration_cast(end - begin).count() << " [ms]"); + gotInput(input, cloudMsgIn.header.frame_id, cloudMsgIn.header.stamp); } void laserScanCallback(const sensor_msgs::msg::LaserScan& scanMsgIn) { - gotInput(PointMatcher_ROS::rosMsgToPointMatcherCloud(scanMsgIn), scanMsgIn.header.frame_id, scanMsgIn.header.stamp); + RCLCPP_DEBUG(this->get_logger(), "----LASER SCAN RECEIVED----"); + std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); + auto input = PointMatcher_ROS::rosMsgToPointMatcherCloud(scanMsgIn); + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Input converted in " << std::chrono::duration_cast(end - begin).count() << " [ms]"); + gotInput(input, scanMsgIn.header.frame_id, scanMsgIn.header.stamp); + } + + void publishAfterInputFilters(const PM::DataPoints& input, const std::string& sensorFrame, const rclcpp::Time& timeStamp) + { + if (inputFiltersScanPublisher->get_subscription_count() > 0) + { + sensor_msgs::msg::PointCloud2 filteredInputMsgOut = PointMatcher_ROS::pointMatcherCloudToRosMsg(input, sensorFrame, timeStamp); + inputFiltersScanPublisher->publish(filteredInputMsgOut); + } + } + + void publishAfterDeskew(const PM::DataPoints& input, const std::string& sensorFrame, const rclcpp::Time& timeStamp) + { + if (deskewingScanPublisher->get_subscription_count() > 0) + { + sensor_msgs::msg::PointCloud2 deskewedCloudMsgOut = PointMatcher_ROS::pointMatcherCloudToRosMsg(input, sensorFrame, timeStamp); + deskewingScanPublisher->publish(deskewedCloudMsgOut); + } } void mapPublisherLoop()