2D positioning error occurred, the positioning program selected the wrong pointcloud2 topic

[ghost]

Dear Carlos,

I tried using your outstanding work and its positioning effect was very excellent. But I have encountered a problem now, and I have tried to modify the code or configuration file, but none of them can solve it. I sincerely hope that you can point out the mistakes I have made.

The problem I encountered is that /ambient_pointcloud is getting farther and farther away from the wall. Then the positioning data also showed a few centimeters of error compared to the actual data. /ambient_pointcloud becomes less attached to the wall as it runs, and then /aligned_pointcloud remains attached to the wall. I believe that the positioning data should be output by /aligned_pointcloud instead of /ambient_pointcloud , but the actual situation is that /ambient_pointcloud and radar /scan are attached, and the positioning data is also published based on /ambient_pointcloud.

What is in red in the picture is /aligned_pointcloud ，and what is in white in the picture is /ambient_pointcloud ，It can be seen that /aligned_pointcloud is in contact with the wall. And /ambient_pointcloud has error. And this error will gradually increase as the car runs, I guess it's due to compensation for the odometer. By the way, I confirmed that the odometer data is accurate.

/ambient_pointcloud and /scan are matched:

Only when I give it another initial position value will everything match and become normal.

This is my TF data, one radar, one Odom:

Here is my configuration file: drl_configs.txt

launch file: launch.txt

What can I do to make the location data based on /aligned_pointcloud ?

Wishing you a wonderful day!

[carlosmccosta]

Hello,

This is the expected behavior of drl. By default, the ambient_pointcloud is on the odom tf frame, which accumulates error over time and the aligned_pointcloud is on the map tf frame, which is the corrected frame (aligned to map).

The pose published in topic localization_pose is based on the aligned_pointcloud and is the same as the tf chain map -> base_link_frame_id (configured on the launch file, which you seem to have set to laser).

Check in rviz with the global frame set to map and visualize the topic localization_pose as an arrow to track the pose estimated by drl. Then move the robot to several map corners and check if the drl pose arrow is where it was expected to be.

Have a nice day,

[ghost]

Thank you for your reply. I have tried adjustments, I changed base_link_frame_id to base_link. but the positioning data still seems to be based on ambient_pointcloud.

The laserscan and ambient_pointcloud fit together, and the localization_pose also coincides with the laser's TF, which is equivalent to saying that the localization_pose is based on the ambient_pointcloud instead of the aligned_pointcloud.

And only after I publish an initial value, the positioning data becomes correct. before : after(The two point clouds overlap and the positioning data becomes correct.):

I guess this may be related to TF, I am trying to solve it,

wish you have a good day. launch.txt drl_configs.txt

[carlosmccosta]

Hello,

It seems you have changed the publish_pose_tf_rate parameter to a positive value.

By default it has a negative value to indicate that the map -> odom tf will only be published in the TF chain after a new pose is estimated by drl (after a new laser scan alignment).

If you specify a positive value, the map -> odom will be published at a fixed rate, using the last alignment correction pose but with an updated time (ros::Time::now()).

This functionality was added to allow other nodes to have available the tf map -> base_link between laser scan alignment corrections, allowing the usage of the odometry temporarily, until the next map -> odom correction is done from the next laser scan.

This might be useful if you have a motion controller running and constantly looking at the latest tf map -> base_link at 100 Hz, but have a laser scanning at 10 Hz, for example. If you specify a positive value of 100 Hz for the publish_pose_tf_rate, the controller will get a valid tf map -> base_link at 100 Hz, but between each scan alignment (10 Hz), it will be relying on the odometry information to know where is the current pose of the robot on the map.

The drawback of this approach is that all the other nodes (including rviz), that look at tf to transform data to the map frame (which includes the laser scans), will have the error associated with odometry between scan alignment corrections.

Without this feature, the motion controller would block when calling the lookup transform api and would only be able to run at 10 Hz.

Alternatively, the motion controller could internally use the last map -> odom correction (10 Hz) and them combine it with the last odometry estimation (100 Hz). This is the best approach, that does not affect other nodes, such as rviz, and allows the motion controller to run at his own rate without being limited by the slow laser scanning.

That is why drl has by default the publish_pose_tf_rate parameter set to a negative value.

If you do not want this odometry error between scan alignment corrections, then you need to set the publish_pose_tf_rate to a negative value.

Also, the drl_configs.yaml is only for documentation.

The yamls loaded to the parameter server and used by drl are the ones specified in the dynamic_robot_localization_system.launch along with the other roslaunch parameters inside the node xml tag.

Have a nice day,

[ghost]

Hello， Thanks for your reply,

I adjusted it as you said, and the drift speed dropped significantly, but when I ran it for more than half an hour, the same drift still occurred.

I suspect that there is something wrong with laser scan assembler , because when I adjust the value of <arg name="number_of_tf_queries_for_spherical_interpolation" default="4" /> to 10, the location drifts a lot after running for two or three minutes, the actual position can be several meters away.

Even if I keep the car stationary, it will still drift slowly.

So I tried to modify the laser scan assembler frame, I set it to laser, than adjust the value of <arg name="number_of_tf_queries_for_spherical_interpolation" default="4" /> to 0, and now everything works fine. Drifting no longer occurs.

    <!-- laser scan assembler -->
    <arg name="use_laser_assembler" default="true" />
    <arg name="assemble_lasers_on_map_frame" default="false" /> <!-- if false -> assembles lasers on odom frame -->
    <arg name="laser_assembly_frame" default="$(arg map_frame_id)" if="$(arg assemble_lasers_on_map_frame)" />
    <arg name="laser_assembly_frame" default="laser" unless="$(arg assemble_lasers_on_map_frame)" />

with you have a good day!

[carlosmccosta]

Hello,

By default the laserscan_to_pointcloud package uses the odom frame with 4 number_of_tf_queries_for_spherical_interpolation to correct for the scan deformation when the robot is moving.

But this feature will only work reliably if you have good odometry. If there is still drift even with the robot stopped, it means you need to recalibrate your odometry. With the robot stopped, the odom TF should stay the same and not change.

In the readme below you have more documentation and examples about the impact of the spherical linear interpolation correction in the final point cloud (generated from the laser scan). https://github.com/carlosmccosta/laserscan_to_pointcloud

In the examples you can see that without the correction the first and last laser measurements do not overlap on the same wall line and on the left of the map the measurements do not overlap with the walls.

If you compare these images with the ones with the correction, you can see that these scan deformation issues are corrected.

Have a nice day,

[ghost]

Hello, I thought the drifting problem had been solved, but it seems not, it just appears much slower.

I have tried various methods in the past few days, such as disabling the laser assembler, replacing other point cloud merges and publishing ambient_pointcloud topic, or debugging the parameters in launch, or calibrating the odometer, but none of these worked, and the slow drift still occurred. It is worth mentioning that there is only a map to odom conversion in tf.

The only way to solve this problem, I have found now, is to publish an init pose, and then trigger the tracking program, the icp registration. I am now trying to run it every thirty seconds.

I am going to modify the logic of your code. I think this is what should trigger the tracking, and this should solve the drifting problem：

wish you have a good day！

[carlosmccosta]

Hello,

I found what was the problem.

This commit added the stop of the point cloud subscribers at the end of the drl pipeline when it exceeded the limit_of_pointclouds_to_process parameter without checking first if the limit was a number bigger than zero. https://github.com/carlosmccosta/dynamic_robot_localization/commit/cac2ffcbf55b28a327a413f6a6da7ac7f39e8f5a#diff-7513768f17e2a1aa47eaba7a68a5e72984c98ca5ff3178706d67f52f24392e6bR2197

By default, limit_of_pointclouds_to_process is set to -1, which means, no limit is applied to the number of point clouds to process by drl.

I fixed this issue in last commit: https://github.com/carlosmccosta/dynamic_robot_localization/commit/e7466e541e5784bd82c3ab31f0134c1c6ec8301b

Without this check for ensuring limit_of_pointclouds_to_process is bigger than zero, drl would process the first lidar scan and then stop processing any future sensor data. In my precommit tests I had set a limit number higher than zero and did not took into account that the default value was -1.

That was why you were seeing the drift over time, because drl would process the first scan after you set the initial pose in rviz and then would stop the point cloud subscribers, which would result in drl stopping estimating the robot pose.

I apologize for the inconvenience and thank you very much for alerting me of this problem.

Please do git stash and git pull on the drl repository, compile and test again.

Have a nice day :)

[ghost]

Hello,

I believe that drl is working well, but the actual situation is that its positioning results always follow the input ambient_pointcloud, not the aligned_pointcloud. I updated your code and it is still like this. Here are the running pictures and launch files

You can see that only after I send the init pose, the ambient_pointcloud and aligned_pointcloud are aligned, and the positioning result is correct. This is what the odometer looks like when it is stationary:

<?xml version="1.0" encoding="UTF-8"?>
<launch>
    <!-- ======================================================= arguments ==================================================== -->
    <!-- initial pose topics -->
    <arg name="pose_topic" default="initial_pose" />
    <arg name="pose_stamped_topic" default="initial_pose_stamped" />
    <arg name="pose_with_covariance_stamped_topic" default="/initialpose" /> <!-- rviz topic -->
    <arg name="pose_with_covariance_stamped_publish_topic" default="localization_pose_with_covariance" /> <!-- rviz topic -->

    <!-- published pose -->
    <arg name="pose_stamped_publish_topic" default="localization_pose" />

    <!-- frame ids -->
    <arg name="map_frame_id" default="map" />
    <arg name="odom_frame_id" default="odom" />
    <arg name="base_link_frame_id" default="base_link" />
    <arg name="sensor_frame_id" default="laser" />

    <!-- main configurations -->
    <arg name="use_ukf_filtering" default="false" />
    <arg name="nodes_namespace" default="dynamic_robot_localization" />
    <arg name="nodes_respawn" default="true" />
    <arg name="drl_localization_node_name" default="drl_localization_node" />
    <arg name="pcl_verbosity_level" default="ALWAYS" /> <!-- VERBOSE | DEBUG | INFO | WARN | ERROR | ALWAYS || ALWAYS -> no console output -->
    <arg name="ros_verbosity_level" default="FATAL" /> <!-- DEBUG | INFO | WARN | ERROR | FATAL -->
    <arg name="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization)/yaml/configs/filters/filters_large_map_2d.yaml" />
     <!--<arg name="yaml_configuration_pose_estimation_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_estimation/initial_pose_estimation_2d.yaml" />-->
    <arg name="yaml_configuration_pose_estimation_filename" default="$(find dynamic_robot_localization)/yaml/configs/empty.yaml" />
    <arg name="yaml_configuration_tracking_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/cluttered_environments_dynamic_fast_2d.yaml" />
    <arg name="yaml_configuration_recovery_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_recovery/recovery_slam_2d.yaml" />
    <arg name="yaml_configuration_covariance_filename" default="$(find dynamic_robot_localization)/yaml/configs/covariance_estimation/covariance_estimation.yaml" if="$(arg use_ukf_filtering)" />
    <arg name="yaml_configuration_covariance_filename" default="$(find dynamic_robot_localization)/yaml/configs/empty.yaml" unless="$(arg use_ukf_filtering)" />

    <!-- reference map data -->
    <arg name="reference_pointcloud_filename" default="" />
    <arg name="reference_pointcloud_preprocessed_save_filename" default="" />
    <arg name="reference_pointcloud_available" default="true" />
    <arg name="reference_pointcloud_type_3d" default="false" />
    <arg name="reference_pointcloud_type" default="2D" unless="$(arg reference_pointcloud_type_3d)"/>
    <arg name="reference_pointcloud_type" default="2D" if="$(arg reference_pointcloud_type_3d)" />
    <arg name="reference_pointcloud_update_mode" default="NoIntegration" if="$(arg reference_pointcloud_available)"/> <!-- NoIntegration | FullIntegration | InliersIntegration | OutliersIntegration -->
    <arg name="reference_pointcloud_update_mode" default="FullIntegration" unless="$(arg reference_pointcloud_available)"/>
    <arg name="reference_pointcloud_keypoints_filename" default="" />
    <arg name="reference_pointcloud_keypoints_save_filename" default="" />
    <arg name="reference_pointcloud_descriptors_filename" default="" />
    <arg name="reference_pointcloud_descriptors_save_filename" default="" />
    <arg name="reference_costmap_topic" default="" if="$(arg reference_pointcloud_type_3d)" />
    <arg name="reference_costmap_topic" default="/map" unless="$(arg reference_pointcloud_type_3d)" />
    <arg name="reference_costmap_topic_updated" default="/map_updated" />
    <arg name="reference_pointcloud_topic" default="reference_pointcloud_updated" if="$(arg reference_pointcloud_type_3d)" /> <!-- OctoMap is configured to use topic 'reference_pointcloud_update' -->
    <arg name="reference_pointcloud_topic" default="" unless="$(arg reference_pointcloud_type_3d)" />

    <!-- sensor data from point cloud topic -->
    <arg name="ambient_pointcloud_topic" default="ambient_pointcloud" />
    <arg name="ambient_pointcloud_topic_disabled_on_startup" default="false" />
    <arg name="imu_topic" default="" />

    <!-- initial pose setup -->
    <arg name="robot_initial_pose_in_base_to_map" default="false" /> <!-- recommendation: use false to allow direct publish of tf between odom -> map -->
    <arg name="robot_initial_pose_available" default="false" />
    <arg name="robot_initial_x" default="0.0" />
    <arg name="robot_initial_y" default="0.0" />
    <arg name="robot_initial_z" default="0.0" />
    <arg name="robot_initial_roll" default="0.0" />
    <arg name="robot_initial_pitch" default="0.0" />
    <arg name="robot_initial_yaw" default="0.0" />

    <!-- laser scan assembler -->
    <arg name="use_laser_assembler" default="true" />
    <arg name="assemble_lasers_on_map_frame" default="false" /> <!-- if false -> assembles lasers on odom frame -->
    <arg name="laser_assembly_frame" default="$(arg map_frame_id)" if="$(arg assemble_lasers_on_map_frame)" />
    <arg name="laser_assembly_frame" default="$(arg odom_frame_id)" unless="$(arg assemble_lasers_on_map_frame)" />
    <arg name="laser_assembly_motion_estimation_source_frame_id" default="" />
    <arg name="laser_assembly_motion_estimation_target_frame_id" default="" />
    <arg name="laser_scan_topics" default="/scan1+/scan2" />
    <arg name="min_range_cutoff_percentage_offset" default="1.00" />
    <arg name="max_range_cutoff_percentage_offset" default="0.95" />
    <arg name="laser_assembler_remove_invalid_measurements" default="true" />
    <arg name="number_of_tf_queries_for_spherical_interpolation" default="0" />
    <arg name="number_of_scans_to_assemble_per_cloud" default="1" />
    <arg name="timeout_for_cloud_assembly" default="0.05" />
    <arg name="dynamic_update_of_assembly_configuration_from_twist_topic" default="" />
    <arg name="dynamic_update_of_assembly_configuration_from_odometry_topic" default="/odom" />
    <arg name="dynamic_update_of_assembly_configuration_from_imu_topic" default="" />
    <arg name="min_number_of_scans_to_assemble_per_cloud" default="1" />
    <arg name="max_number_of_scans_to_assemble_per_cloud" default="1" />
    <arg name="min_timeout_seconds_for_cloud_assembly" default="0.1" />
    <arg name="max_timeout_seconds_for_cloud_assembly" default="0.1" />
    <arg name="max_linear_velocity" default="0.025" /> <!-- when the linear velocity of the robot is equal or greater than this value, the number of scans to join and assembly timeout will be set to their minimum values specified above -->
    <arg name="max_angular_velocity" default="0.017" /> <!-- when the angular velocity of the robot is equal or greater than this value, the number of scans to join and assembly timeout will be set to their minimum values specified above -->

    <!-- tf configurations -->
    <arg name="publish_tf_map_odom" default="true" />
    <arg name="publish_initial_pose" default="true" />
    <arg name="publish_pose_tf_rate" default="-10" />
    <arg name="tf_lookup_timeout" default="0.5" />
    <arg name="publish_last_pose_tf_timeout_seconds" default="-50" /> <!-- set to negative value for publishing a tf only when drl estimates a new pose -->
    <arg name="invert_tf_transform" default="false" />
    <arg name="invert_tf_hierarchy" default="false" />
    <arg name="transform_pose_to_map_frame_id" default="true" />

    <!-- map update -->
    <arg name="use_octomap_for_dynamic_map_update" default="false" />
    <arg name="use_octomap_only_to_build_occupancy_grid" default="false" />
    <arg name="octomap_pointcloud_topic" default="aligned_pointcloud" />  <!-- ambient_pointcloud | aligned_pointcloud | aligned_pointcloud_outliers | aligned_pointcloud_inliers -->
    <arg name="octomap_file" default="" />
    <arg name="octomap_sensor_frame_id" default="$(arg sensor_frame_id)" />
    <arg name="octomap_resolution" default="0.02" />
    <arg name="octomap_minimum_amount_of_time_between_ros_msg_publishing" default="5.0" />
    <arg name="octomap_minimum_number_of_integrations_before_ros_msg_publishing" default="10" />
    <arg name="octomap_override_sensor_z" default="false" if="$(arg reference_pointcloud_type_3d)" />
    <arg name="octomap_override_sensor_z" default="true" unless="$(arg reference_pointcloud_type_3d)" />
    <arg name="octomap_override_sensor_z_value" default="0.0" />
    <arg name="octomap_cloud_out_topic" default="reference_pointcloud_updated" />
    <arg name="initial_2d_map_file" default="" />

    <!-- ==================================================== localization system ============================================= -->
    <!-- <node pkg="dynamic_robot_localization" type="drl_localization_node" name="$(arg drl_localization_node_name)" respawn="$(arg nodes_respawn)" ns="$(arg nodes_namespace)" clear_params="true" output="screen" launch-prefix="gdbserver localhost:1337" > -->
    <node pkg="dynamic_robot_localization" type="drl_localization_node" name="$(arg drl_localization_node_name)" respawn="$(arg nodes_respawn)" ns="$(arg nodes_namespace)" clear_params="true" output="screen" >
        <param name="pcl_verbosity_level" type="str" value="$(arg pcl_verbosity_level)" />
        <param name="ros_verbosity_level" type="str" value="$(arg ros_verbosity_level)" />
        <param name="message_management/tf_timeout" type="double" value="$(arg tf_lookup_timeout)" />
        <param name="frame_ids/map_frame_id" type="str" value="$(arg map_frame_id)" />
        <param name="frame_ids/odom_frame_id" type="str" value="$(arg odom_frame_id)" />
        <param name="frame_ids/base_link_frame_id" type="str" value="$(arg base_link_frame_id)" />
        <param name="frame_ids/sensor_frame_id" type="str" value="$(arg sensor_frame_id)" />
        <param name="initial_pose/robot_initial_pose_in_base_to_map" type="bool" value="$(arg robot_initial_pose_in_base_to_map)" />
        <param name="initial_pose/robot_initial_pose_available" type="bool" value="$(arg robot_initial_pose_available)" />
        <param name="initial_pose/position/x" type="double" value="$(arg robot_initial_x)" />
        <param name="initial_pose/position/y" type="double" value="$(arg robot_initial_y)" />
        <param name="initial_pose/position/z" type="double" value="$(arg robot_initial_z)" />
        <param name="initial_pose/orientation_rpy/roll" type="double" value="$(arg robot_initial_roll)" />
        <param name="initial_pose/orientation_rpy/pitch" type="double" value="$(arg robot_initial_pitch)" />
        <param name="initial_pose/orientation_rpy/yaw" type="double" value="$(arg robot_initial_yaw)" />
        <param name="subscribe_topic_names/pose_topic" type="str" value="$(arg pose_topic)" />
        <param name="subscribe_topic_names/pose_stamped_topic" type="str" value="$(arg pose_stamped_topic)" />
        <param name="subscribe_topic_names/pose_with_covariance_stamped_topic" type="str" value="$(arg pose_with_covariance_stamped_topic)" />
        <param name="subscribe_topic_names/ambient_pointcloud_topic" type="str" value="$(arg ambient_pointcloud_topic)" />
        <param name="subscribe_topic_names/ambient_pointcloud_topic_disabled_on_startup" type="bool" value="$(arg ambient_pointcloud_topic_disabled_on_startup)" />
        <param name="subscribe_topic_names/reference_costmap_topic" type="str" value="$(arg reference_costmap_topic)" />
        <param name="subscribe_topic_names/reference_pointcloud_topic" type="str" value="$(arg reference_pointcloud_topic)" />
        <param name="publish_topic_names/pose_with_covariance_stamped_publish_topic" type="str" value="$(arg pose_with_covariance_stamped_publish_topic)" />
        <param name="publish_topic_names/pose_stamped_publish_topic" type="str" value="$(arg pose_stamped_publish_topic)" />
        <param name="reference_pointclouds/reference_pointcloud_filename" type="str" value="$(arg reference_pointcloud_filename)" />
        <param name="reference_pointclouds/reference_pointcloud_preprocessed_save_filename" type="str" value="$(arg reference_pointcloud_preprocessed_save_filename)" />
        <param name="reference_pointclouds/reference_pointcloud_type" type="str" value="$(arg reference_pointcloud_type)" />
        <param name="reference_pointclouds/reference_pointcloud_available" type="bool" value="$(arg reference_pointcloud_available)" />
        <param name="reference_pointclouds/reference_pointcloud_update_mode" type="str" value="$(arg reference_pointcloud_update_mode)" />
        <param name="keypoint_detectors/reference_pointcloud/reference_pointcloud_keypoints_filename" type="str" value="$(arg reference_pointcloud_keypoints_filename)" />
        <param name="keypoint_detectors/reference_pointcloud/reference_pointcloud_keypoints_save_filename" type="str" value="$(arg reference_pointcloud_keypoints_save_filename)" />
        <param name="initial_pose_estimators_matchers/feature_matchers/reference_pointcloud_descriptors_filename" type="str" value="$(arg reference_pointcloud_descriptors_filename)" />
        <param name="initial_pose_estimators_matchers/feature_matchers/reference_pointcloud_descriptors_save_filename" type="str" value="$(arg reference_pointcloud_descriptors_save_filename)" />
        <rosparam command="load" file="$(arg yaml_configuration_filters_filename)" subst_value="true" />
        <rosparam command="load" file="$(arg yaml_configuration_pose_estimation_filename)" subst_value="false" />
        <rosparam command="load" file="$(arg yaml_configuration_recovery_filename)" subst_value="true" />
        <rosparam command="load" file="$(arg yaml_configuration_tracking_filename)" subst_value="true" />
        <rosparam command="load" file="$(arg yaml_configuration_covariance_filename)" subst_value="true" />
    </node>

    <include file="$(find dynamic_robot_localization)/launch/ukf.launch" ns="$(arg nodes_namespace)" if="$(arg use_ukf_filtering)" >
        <arg name="planar_pose_estimation" default="false" if="$(arg reference_pointcloud_type_3d)" />
        <arg name="planar_pose_estimation" default="true" unless="$(arg reference_pointcloud_type_3d)" />
        <arg name="tracking_state_reset_topic" default="initial_pose_with_covariance" />
        <arg name="sensor_timeout" default="$(arg publish_last_pose_tf_timeout_seconds)" />
        <arg name="map_frame_id" default="$(arg map_frame_id)" />
        <arg name="odom_frame_id" default="$(arg odom_frame_id)" />
        <arg name="base_link_frame_id" default="$(arg base_link_frame_id)" />
        <arg name="odometry_topic" default="$(arg dynamic_update_of_assembly_configuration_from_odometry_topic)" />
        <arg name="imu_topic" default="$(arg imu_topic)" />
        <arg name="nodes_respawn" default="$(arg nodes_respawn)" />
    </include>

    <!-- map to odom publisher -->
    <include file="$(find pose_to_tf_publisher)/launch/pose_to_tf_publisher.launch" if="$(arg publish_tf_map_odom)" >
        <arg name="publish_rate" default="$(arg publish_pose_tf_rate)" />
        <arg name="tf_lookup_timeout" default="1.5" />
        <arg name="publish_last_pose_tf_timeout_seconds" default="$(arg publish_last_pose_tf_timeout_seconds)" />
        <arg name="pose_with_covariance_stamped_topic" default="$(arg pose_with_covariance_stamped_topic)" /> <!-- rviz topic -->
        <arg name="pose_stamped_topic" default="$(arg pose_stamped_publish_topic)" />
        <arg name="odometry_topic" default="odom" />
        <arg name="map_frame_id" default="$(arg map_frame_id)_drl" if="$(arg use_ukf_filtering)" />
        <arg name="map_frame_id" default="$(arg map_frame_id)" unless="$(arg use_ukf_filtering)" />
        <arg name="odom_frame_id" default="$(arg odom_frame_id)" />
        <arg name="base_link_frame_id" default="$(arg base_link_frame_id)" />
        <arg name="publish_initial_pose" default="$(arg publish_initial_pose)" />
        <arg name="initial_pose_in_base_to_map" default="$(arg robot_initial_pose_in_base_to_map)" />
        <arg name="initial_x" default="$(arg robot_initial_x)" />
        <arg name="initial_y" default="$(arg robot_initial_y)" />
        <arg name="initial_z" default="$(arg robot_initial_z)" />
        <arg name="initial_roll" default="$(arg robot_initial_roll)" />
        <arg name="initial_pitch" default="$(arg robot_initial_pitch)" />
        <arg name="initial_yaw" default="$(arg robot_initial_yaw)" />
        <arg name="invert_tf_transform" default="true" if="$(arg use_ukf_filtering)" />
        <arg name="invert_tf_transform" default="$(arg invert_tf_transform)" unless="$(arg use_ukf_filtering)" />
        <arg name="invert_tf_hierarchy" default="true" if="$(arg use_ukf_filtering)" />
        <arg name="invert_tf_hierarchy" default="$(arg invert_tf_hierarchy)" unless="$(arg use_ukf_filtering)" />
        <arg name="transform_pose_to_map_frame_id" default="false" if="$(arg use_ukf_filtering)" />
        <arg name="transform_pose_to_map_frame_id" default="$(arg transform_pose_to_map_frame_id)" unless="$(arg use_ukf_filtering)" />
        <arg name="parameters_namespace" default="dynamic_robot_localization" />
        <arg name="nodes_respawn" default="$(arg nodes_respawn)" />
    </include>

    <!-- laser assembler -->
    <include file="$(find laserscan_to_pointcloud)/launch/laserscan_to_pointcloud_assembler.launch" ns="$(arg nodes_namespace)" if="$(arg use_laser_assembler)" >
        <arg name="laser_scan_topics" default="$(arg laser_scan_topics)" />
        <arg name="number_of_tf_queries_for_spherical_interpolation" default="$(arg number_of_tf_queries_for_spherical_interpolation)" />
        <arg name="number_of_scans_to_assemble_per_cloud" default="$(arg number_of_scans_to_assemble_per_cloud)" />
        <arg name="timeout_for_cloud_assembly" default="$(arg timeout_for_cloud_assembly)" />
        <arg name="remove_invalid_measurements" default="$(arg laser_assembler_remove_invalid_measurements)" />
        <arg name="dynamic_update_of_assembly_configuration_from_twist_topic" default="$(arg dynamic_update_of_assembly_configuration_from_twist_topic)" />
        <arg name="dynamic_update_of_assembly_configuration_from_odometry_topic" default="$(arg dynamic_update_of_assembly_configuration_from_odometry_topic)" />
        <arg name="dynamic_update_of_assembly_configuration_from_imu_topic" default="$(arg dynamic_update_of_assembly_configuration_from_imu_topic)" />
        <arg name="min_range_cutoff_percentage_offset" default="$(arg min_range_cutoff_percentage_offset)" />
        <arg name="max_range_cutoff_percentage_offset" default="$(arg max_range_cutoff_percentage_offset)" />
        <arg name="min_number_of_scans_to_assemble_per_cloud" default="$(arg min_number_of_scans_to_assemble_per_cloud)" />
        <arg name="max_number_of_scans_to_assemble_per_cloud" default="$(arg max_number_of_scans_to_assemble_per_cloud)" />
        <arg name="min_timeout_seconds_for_cloud_assembly" default="$(arg min_timeout_seconds_for_cloud_assembly)" />
        <arg name="max_timeout_seconds_for_cloud_assembly" default="$(arg max_timeout_seconds_for_cloud_assembly)" />
        <arg name="max_linear_velocity" default="$(arg max_linear_velocity)" />
        <arg name="max_angular_velocity" default="$(arg max_angular_velocity)" />
        <arg name="target_frame" default="$(arg laser_assembly_frame)" />
        <arg name="recovery_frame" default="$(arg odom_frame_id)" if="$(arg assemble_lasers_on_map_frame)" />
        <arg name="recovery_frame" default="" unless="$(arg assemble_lasers_on_map_frame)" />
        <arg name="initial_recovery_transform_in_base_link_to_target" default="$(arg robot_initial_pose_in_base_to_map)" />
        <arg name="base_link_frame_id" default="$(arg base_link_frame_id)" />
        <arg name="motion_estimation_source_frame_id" default="$(arg laser_assembly_motion_estimation_source_frame_id)" />
        <arg name="motion_estimation_target_frame_id" default="$(arg laser_assembly_motion_estimation_target_frame_id)" />
        <arg name="recovery_to_target_frame_transform_initial_x" default="$(arg robot_initial_x)" />
        <arg name="recovery_to_target_frame_transform_initial_y" default="$(arg robot_initial_y)" />
        <arg name="recovery_to_target_frame_transform_initial_z" default="$(arg robot_initial_z)" />
        <arg name="recovery_to_target_frame_transform_initial_roll" default="$(arg robot_initial_roll)" />
        <arg name="recovery_to_target_frame_transform_initial_pitch" default="$(arg robot_initial_pitch)" />
        <arg name="recovery_to_target_frame_transform_initial_yaw" default="$(arg robot_initial_yaw)" />
        <arg name="nodes_respawn" default="$(arg nodes_respawn)" />
    </include>

    <!-- OctoMap for dynamic map update -->
    <group if="$(arg use_octomap_for_dynamic_map_update)" ns="$(arg nodes_namespace)" >
        <include file="$(find dynamic_robot_localization)/launch/octo_map.launch" >
            <arg name="octomap_file" default="$(arg octomap_file)" />
            <arg name="cloud_in" default="$(arg octomap_pointcloud_topic)" />
            <arg name="map_frame_id" default="$(arg map_frame_id)" />
            <arg name="base_frame_id" default="$(arg base_link_frame_id)" />
            <arg name="sensor_frame_id" default="$(arg octomap_sensor_frame_id)" />
            <arg name="override_sensor_z" default="$(arg octomap_override_sensor_z)" />
            <arg name="override_sensor_z_value" default="$(arg octomap_override_sensor_z_value)" />
            <arg name="resolution" default="$(arg octomap_resolution)" />
            <arg name="minimum_amount_of_time_between_ros_msg_publishing" default="$(arg octomap_minimum_amount_of_time_between_ros_msg_publishing)" />
            <arg name="minimum_number_of_integrations_before_ros_msg_publishing" default="$(arg octomap_minimum_number_of_integrations_before_ros_msg_publishing)" />
            <arg name="cloud_out_topic" default="$(arg octomap_cloud_out_topic)" />
            <arg name="map_out_topic" default="$(arg reference_costmap_topic_updated)" if="$(arg use_octomap_only_to_build_occupancy_grid)" />
            <arg name="map_out_topic" default="$(arg reference_costmap_topic)" unless="$(arg use_octomap_only_to_build_occupancy_grid)" />
            <arg name="nodes_respawn" default="$(arg nodes_respawn)" />
        </include>

        <group if="$(arg reference_pointcloud_available)" >
            <node name="map_server" pkg="map_server" type="map_server" args="$(arg initial_2d_map_file)" respawn="$(arg nodes_respawn)" clear_params="true" output="screen" unless="$(arg reference_pointcloud_type_3d)" >
                <param name="frame_id" type="str" value="$(arg map_frame_id)" />
                <remap from="map" to="initial_2d_map" />
            </node>
        </group>
    </group>

</launch>

have a nice day

[ghost]

Hello! I have tested DRL for a long time. It seems to be suitable for scenes with many moving obstacles. The effect is much better than most 2D positioning algorithms. But the drift of tf does not seem to trigger the correction program. Is the method of initial registration of point clouds the same as the registration method during operation? And is there any way for DRL to register two point clouds without sending initial values?