dynamic_robot_localization
dynamic_robot_localization copied to clipboard
carlosmccosta
2D localization mode have roll pitch and Zaxis drit
Hello,
For 3 DoF localization (x, y, yaw), you must use the dynamic_robot_localization_system.launch, which is configured for using 2D lidars and 2D occupancy grids.
drl assumes that your lidar is mounted parallel to the floor and after transforming the lidar data to the odom TF, it will set the points Z coordinate to zero.
If your lidar is not parallel to the floor, you will not be able to use 3 DoF localization algorithms and maps, because they all assume sensors parallel to the floor to ensure that when the sensor changes place within the map, the measurements remain consistent.
Have a nice day,
@carlosmccosta hello, I do use the [dynamic_robot_localization_system.launch],and below is the launch parameter.
<!-- 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="laser" />
<arg name="sensor_frame_id" default="laser" />
<!-- main configurations -->
<arg name="use_ukf_filtering" default="false" />
<arg name="nodes_namespace" default="drl_slam" />
<arg name="nodes_respawn" default="true" />
<arg name="localization" default="localization" />
<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 super_slam)/yaml/configs/filters/filters_large_map_2d.yaml" />
<arg name="yaml_configuration_pose_estimation_filename" default="$(find super_slam)/yaml/configs/empty.yaml" />
<arg name="yaml_configuration_tracking_filename" default="$(find super_slam)/yaml/configs/pose_tracking/cluttered_environments_dynamic_fast_2d.yaml" />
<arg name="yaml_configuration_recovery_filename" default="$(find super_slam)/yaml/configs/pose_recovery/recovery_slam_2d.yaml" />
<arg name="yaml_configuration_covariance_filename" default="$(find super_slam)/yaml/configs/covariance_estimation/covariance_estimation.yaml" if="$(arg use_ukf_filtering)" />
<arg name="yaml_configuration_covariance_filename" default="$(find super_slam)/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="3D" 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="/imu/data" />
<!-- 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="/scan" />
<arg name="min_range_cutoff_percentage_offset" default="2.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="4" />
<arg name="number_of_scans_to_assemble_per_cloud" default="1" />
<arg name="timeout_for_cloud_assembly" default="0.5" />
<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="/imu/data" />
<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.15" />
<arg name="max_timeout_seconds_for_cloud_assembly" default="0.15" />
<arg name="max_linear_velocity" default="0.0001" /> <!-- 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.0001" /> <!-- 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.0" />
<arg name="tf_lookup_timeout" default="0.1" />
<arg name="publish_last_pose_tf_timeout_seconds" default="-330.0" /> <!-- 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="super_slam" type="localization" name="$(arg localization)" respawn="$(arg nodes_respawn)" ns="$(arg nodes_namespace)" clear_params="true" output="screen" launch-prefix="gdbserver localhost:1337" > -->
<node pkg="super_slam" type="localization" name="$(arg localization)" 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 super_slam)/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="" />
<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="super_slam" />
<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 super_slam)/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>
Hello,
Check if the roll and pitch angles between the TFs odom and laser are 0 degrees. The 2d icp configured in drl assumes that the laser is parallel to the ground and uses the pcl transformation_estimation_2D
Check your ROS parameter server (rosparam get / -p) to confirm that the 2d icp was correctly loaded.
Looking at your rviz screenshot it seems that the laser is tilted and not parallel to the ground.
Have a nice day :)
hello @carlosmccosta this is my param,please check if anything wrong,I need a consecutive pose for navigation so I subscribed the tf (map->laser),I also found that the tf have x y yaw drift and increase the drift with time,but the pose from topic doesn't have drift,and the tf drift will be clean when drl received /initpose topic.
dynamic_robot_localization: drl_localization_node: cloud_analyzers: angular_distribution_analyzer: number_of_angular_bins: 270 compute_inliers_angular_distribution: true compute_outliers_angular_distribution: true filters: ambient_pointcloud: 1_voxel_grid: downsample_all_data: false filter_limit_field_name: z filter_limit_max: 2.0 filter_limit_min: -1.0 filtered_cloud_publish_topic: leaf_size_x: 0.02 leaf_size_y: 0.02 leaf_size_z: 0.02 save_leaf_layout: false 5_random_sample: filtered_cloud_publish_topic: ambient_pointcloud_filtered invert_sampling: false number_of_random_samples: 2500 reference_pointcloud: 1_voxel_grid: downsample_all_data: false filter_limit_field_name: z filter_limit_max: 1.8 filter_limit_min: -0.001 filtered_cloud_publish_topic: leaf_size_x: 0.02 leaf_size_y: 0.02 leaf_size_z: 0.02 save_leaf_layout: false frame_ids: base_link_frame_id: laser map_frame_id: map odom_frame_id: odom sensor_frame_id: laser initial_pose: orientation_rpy: pitch: 0.0 roll: 0.0 yaw: 0.0 position: x: 0.0 y: 0.0 z: 0.0 robot_initial_pose_available: false robot_initial_pose_in_base_to_map: false initial_pose_estimators_matchers: feature_matchers: reference_pointcloud_descriptors_filename: reference_pointcloud_descriptors_save_filename: keypoint_detectors: reference_pointcloud: reference_pointcloud_keypoints_filename: reference_pointcloud_keypoints_save_filename: localization_point_type: PointXYZINormal message_management: tf_timeout: 0.1 outlier_detectors: euclidean_outlier_detector: aligned_pointcloud_inliers_publish_topic: /aligned_pointcloud_inliers aligned_pointcloud_outliers_publish_topic: /aligned_pointcloud_outliers max_inliers_distance: 0.06 pcl_verbosity_level: ALWAYS publish_topic_names: pose_stamped_publish_topic: localization_pose pose_with_covariance_stamped_publish_topic: localization_pose_with_covariance reference_pointclouds: reference_pointcloud_available: true reference_pointcloud_filename: reference_pointcloud_preprocessed_save_filename: reference_pointcloud_type: 2D reference_pointcloud_update_mode: NoIntegration ros_verbosity_level: FATAL subscribe_topic_names: ambient_pointcloud_topic: ambient_pointcloud ambient_pointcloud_topic_disabled_on_startup: false pose_stamped_topic: initial_pose_stamped pose_topic: initial_pose pose_with_covariance_stamped_topic: /initialpose reference_costmap_topic: /map reference_pointcloud_topic: tracking_matchers: ignore_height_corrections: false point_matchers: iterative_closest_point_2d: correspondence_randomness: 50 display_cloud_aligment: false euclidean_fitness_epsilon: 0.0001 match_only_keypoints: false max_correspondence_distance: 0.2 max_number_of_ransac_iterations: 140 max_number_of_registration_iterations: 140 maximum_number_of_displayed_correspondences: 0 maximum_optimizer_iterations: 150 ransac_outlier_rejection_threshold: 0.06 rotation_epsilon: 0.002 transformation_epsilon: 0.0001 use_reciprocal_correspondences: false tracking_recovery_matchers: point_matchers: iterative_closest_point: convergence_time_limit_seconds: 0.75 correspondence_randomness: 50 display_cloud_aligment: false euclidean_fitness_epsilon: 1.0e-06 match_only_keypoints: false max_correspondence_distance: 1.75 max_number_of_ransac_iterations: 200 max_number_of_registration_iterations: 300 maximum_number_of_displayed_correspondences: 0 maximum_optimizer_iterations: 100 ransac_outlier_rejection_threshold: 0.06 rotation_epsilon: 0.002 transformation_epsilon: 1.0e-06 use_reciprocal_correspondences: false transformation_validators: euclidean_transformation_validator: max_new_pose_diff_angle: 0.45 max_new_pose_diff_distance: 0.45 max_outliers_percentage: 0.7 max_root_mean_square_error: 0.045 max_transformation_angle: 0.45 max_transformation_distance: 0.45 transformation_validators_tracking_recovery: euclidean_transformation_validator: max_new_pose_diff_angle: 0.8 max_new_pose_diff_distance: 0.8 max_outliers_percentage: 0.7 max_root_mean_square_error: 0.045 max_transformation_angle: 0.8 max_transformation_distance: 0.8 laserscan_to_pointcloud_assembler_jason_2342_5905162112553559908: base_link_frame_id: laser dynamic_update_of_assembly_configuration_from_imu_topic: dynamic_update_of_assembly_configuration_from_odometry_topic: /odom dynamic_update_of_assembly_configuration_from_twist_topic: enforce_reception_of_laser_scans_in_all_topics: true include_laser_intensity: true initial_recovery_transform_in_base_link_to_target: false laser_frame: laser_scan_topics: /scan max_angular_velocity: 0.0001 max_linear_velocity: 0.0001 max_number_of_scans_to_assemble_per_cloud: 1 max_range_cutoff_percentage_offset: 0.95 max_timeout_seconds_for_cloud_assembly: 0.15 min_number_of_scans_to_assemble_per_cloud: 1 min_range_cutoff_percentage_offset: 2.0 min_timeout_seconds_for_cloud_assembly: 0.15 motion_estimation_source_frame_id: motion_estimation_target_frame_id: number_of_scans_to_assemble_per_cloud: 1 number_of_tf_queries_for_spherical_interpolation: 4 pointcloud_publish_topic: ambient_pointcloud recovery_frame: recovery_to_target_frame_transform_initial_pitch: 0.0 recovery_to_target_frame_transform_initial_roll: 0.0 recovery_to_target_frame_transform_initial_x: 0.0 recovery_to_target_frame_transform_initial_y: 0.0 recovery_to_target_frame_transform_initial_yaw: 0.0 recovery_to_target_frame_transform_initial_z: 0.0 remove_invalid_measurements: true target_frame: odom tf_lookup_timeout: 0.15 timeout_for_cloud_assembly: 0.5 pose_to_tf_publisher_node_jason_2342_2149551555346644467: base_link_frame_id: laser discard_older_poses: true float_topic: float_update_field: float_update_field_orientation_in_degrees: false initial_pitch: 0.0 initial_pose_in_base_to_map: false initial_roll: 0.0 initial_x: 0.0 initial_y: 0.0 initial_yaw: 0.0 initial_z: 0.0 invert_tf_hierarchy: false invert_tf_transform: false map_frame_id: map odom_frame_id: odom odometry_topic: pose_stamped_topic: localization_pose pose_with_covariance_stamped_topic: /initialpose poses_filename: publish_initial_pose: true publish_last_pose_tf_timeout_seconds: -3330.0 publish_rate: 3000.0 tf_lookup_timeout: 1.5 tf_time_offset: 0.0 transform_pose_to_map_frame_id: true transform_tf_message_source: transform_tf_message_target:
Hello,
What is your TF chain configuration from map_frame_id to base_link_frame_id ?
From your previous launch files, I assumed that it was map -> odom -> laser
If your odom is not accurate, you can disable its usage within drl by:
- Setting the odom_frame_id to a value equal to the base_link_frame_id
- Since a TF frame can only have one parent in the TF tree, for bypassing the
odom_frame_id, it is necessary to:- Set to
truethe flag invert_tf_transform - Set to
truethe flag invert_tf_hierarchy - This way, the TF and pose_stamped_publish_topic will be published directly from base_link_frame_id to map_frame_id (the pose is specified in the
map_frame_idcoordinate system) - You can check the
drlpose and TF in rviz with the fixed frame configured tomapand can also check in the console with:rostopic echo /dynamic_robot_localization/localization_poserosrun tf tf_echo map laser _rate:=30 _precision:=4
- Set to
I rechecked one of my validation testes from a rosbag (freiburg2_pioneer_slam1.launch) and the latest commit of drl in the noetic-devel branch is performing as expected and without drift, as shown in this video:
https://www.youtube.com/watch?v=jAJ5wiN-mJ8
Have a nice day,
hello @carlosmccosta I found another problem from drl,my odom is accurate so I read the map->laser tf to navigation,and the pose will jump to far a way if the map changed alot and the initial_pose_estimation is set,so I disabled the initial_pose_estimation.but some time it wouldn‘t be able to find it self in the map even the environment is not so bad,I also tune some param from pose_recovery file,so does the initial_pose_estimation will help this situation？or how to tune the initial_pose_estimation so the pose will not jump to far?
@carlosmccosta below is the param I set initial_pose_estimation_2d*********************************** normal_estimators: reference_pointcloud: display_normals: false display_occupancy_grid_pointcloud: false flip_normals_using_occupancy_grid_analysis: true occupancy_grid_analysis_radius_resolution_percentage: 4.0 normal_estimator_sac: model_type: 'SACMODEL_LINE' method_type: 'SAC_RANSAC' inlier_distance_threshold: 0.06 max_iterations: 50 sac_termination_probability: 0.99 optimize_coefficients: true min_model_radius: -1.0 max_model_radius: -1.0 random_samples_max_k: 0 random_samples_max_radius: 0.06 minimum_inliers_percentage: 0.5 ambient_pointcloud: compute_normals_when_tracking_pose: false compute_normals_when_recovering_pose_tracking: false compute_normals_when_estimating_initial_pose: true display_normals: false normal_estimator_sac: model_type: 'SACMODEL_LINE' method_type: 'SAC_RANSAC' inlier_distance_threshold: 0.06 max_iterations: 50 sac_termination_probability: 0.99 optimize_coefficients: true min_model_radius: -1.0 max_model_radius: -1.0 random_samples_max_k: 0 random_samples_max_radius: 0.065 minimum_inliers_percentage: 0.5
curvature_estimators: reference_pointcloud: principal_curvatures_estimation: colorize_pointcloud_with_curvatures: true search_k: 0 search_radius: 0.06 curvature_type: 'CURVATURE_TYPE_MEAN' update_normals_with_principal_component_directions: false ambient_pointcloud: principal_curvatures_estimation: colorize_pointcloud_with_curvatures: true search_k: 0 search_radius: 0.06 curvature_type: 'CURVATURE_TYPE_MEAN' update_normals_with_principal_component_directions: false
keypoint_detectors: reference_pointcloud: sift_3d: min_scale: 0.015 number_octaves: 4 number_scales_per_octave: 8 min_contrast: 0.0035 sift3d_keypoints_cloud_publish_topic: 'reference_keypoints' ambient_pointcloud: compute_keypoints_when_tracking_pose: false compute_keypoints_when_recovering_pose_tracking: false compute_keypoints_when_estimating_initial_pose: true sift_3d: min_scale: 0.015 number_octaves: 4 number_scales_per_octave: 8 min_contrast: 0.0035 sift3d_keypoints_cloud_publish_topic: 'ambient_keypoints'
initial_pose_estimators_matchers: feature_matchers: display_feature_matching: false save_descriptors_in_binary_format : true keypoint_descriptors: feature_descriptor_k_search: 0 feature_descriptor_radius_search: 0.07 fpfh: number_subdivisions_f1: 11 number_subdivisions_f2: 11 number_subdivisions_f3: 11
matchers:
max_correspondence_distance: 0.3
max_number_of_registration_iterations: 2500
sample_consensus_initial_alignment_prerejective:
convergence_time_limit_seconds: 3.0
similarity_threshold: 0.6
inlier_fraction: 0.75
inlier_rmse: 0.2
number_of_samples: 32
correspondence_randomness: 5
point_matchers:
iterative_closest_point:
convergence_time_limit_seconds: 2.0
max_correspondence_distance: 10.0
transformation_epsilon: 0.0001
euclidean_fitness_epsilon: 0.0001
max_number_of_registration_iterations: 350
max_number_of_ransac_iterations: 350
ransac_outlier_rejection_threshold: 0.05
match_only_keypoints: false
display_cloud_aligment: false
maximum_number_of_displayed_correspondences: 0
rotation_epsilon: 0.002
correspondence_randomness: 50
maximum_optimizer_iterations: 100
use_reciprocal_correspondences: false
transformation_validators_initial_alignment: euclidean_transformation_validator: max_transformation_angle: -1.0 max_transformation_distance: 3 max_new_pose_diff_angle: -1.0 max_new_pose_diff_distance: 3 max_root_mean_square_error: -1.0 max_outliers_percentage: 0.5
cluttered_environments_dynamic_fast_2d* tracking_matchers: ignore_height_corrections: false point_matchers: iterative_closest_point_2d: max_correspondence_distance: 0.05 transformation_epsilon: 0.0001 euclidean_fitness_epsilon: 0.0001 max_number_of_registration_iterations: 140 max_number_of_ransac_iterations: 140 ransac_outlier_rejection_threshold: 0.05 match_only_keypoints: false display_cloud_aligment: false maximum_number_of_displayed_correspondences: 0 rotation_epsilon: 0.002 correspondence_randomness: 50 maximum_optimizer_iterations: 150 use_reciprocal_correspondences: false
outlier_detectors: euclidean_outlier_detector: max_inliers_distance: 0.05 aligned_pointcloud_outliers_publish_topic: '/aligned_pointcloud_outliers' aligned_pointcloud_inliers_publish_topic: '/aligned_pointcloud_inliers'
cloud_analyzers: compute_inliers_angular_distribution: true compute_outliers_angular_distribution: true angular_distribution_analyzer: number_of_angular_bins: 270
transformation_validators: euclidean_transformation_validator: max_transformation_angle: 0.3 max_transformation_distance: 0.1 max_new_pose_diff_angle: 0.3 max_new_pose_diff_distance: 0.1 max_root_mean_square_error: 0.1 max_outliers_percentage: 0.66
transformation_validators_tracking_recovery: euclidean_transformation_validator: max_root_mean_square_error: 0.1 max_outliers_percentage: 0.66
recovery_slam_2d****************** tracking_recovery_matchers: point_matchers: iterative_closest_point: convergence_time_limit_seconds: 0.75 max_correspondence_distance: 1.0 transformation_epsilon: 0.000001 euclidean_fitness_epsilon: 0.000001 max_number_of_registration_iterations: 300 max_number_of_ransac_iterations: 200 ransac_outlier_rejection_threshold: 0.04 match_only_keypoints: false display_cloud_aligment: false maximum_number_of_displayed_correspondences: 0 rotation_epsilon: 0.002 correspondence_randomness: 50 maximum_optimizer_iterations: 100 use_reciprocal_correspondences: false
transformation_validators_tracking_recovery: euclidean_transformation_validator: max_transformation_angle: 0.5 max_transformation_distance: 0.2 max_new_pose_diff_angle: 0.5 max_new_pose_diff_distance: 0.2 max_root_mean_square_error: 0.045 max_outliers_percentage: 0.66
filters_large_map_2d.yaml*************** localization_point_type: 'PointXYZINormal'
filters: reference_pointcloud: 1_voxel_grid: leaf_size_x: 0.02 leaf_size_y: 0.02 leaf_size_z: 0.02 filter_limit_field_name: 'z' filter_limit_min: -0.001 filter_limit_max: 1.8 filtered_cloud_publish_topic: '' downsample_all_data: false save_leaf_layout: false ambient_pointcloud: 1_voxel_grid: leaf_size_x: 0.02 leaf_size_y: 0.02 leaf_size_z: 0.02 filter_limit_field_name: 'z' filter_limit_min: -1.0 filter_limit_max: 2.0 filtered_cloud_publish_topic: '' downsample_all_data: false save_leaf_layout: false 5_random_sample: number_of_random_samples: 2500 invert_sampling: false filtered_cloud_publish_topic: 'ambient_pointcloud_filtered'
*****covariance_estimation.yaml registration_covariance_estimator: error_metric: 'PointToPlane3D' correspondance_estimator: 'CorrespondenceEstimation' correspondence_distance_threshold: 0.1 sensor_std_dev_noise: 0.015 use_reciprocal_correspondences: false filtered_reference_cloud_publish_topic: 'covariance_reference_cloud' filtered_ambient_cloud_publish_topic: 'covariance_ambient_cloud' random_sample: number_of_random_samples: 300 invert_sampling: false filtered_cloud_publish_topic: ''
Hello,
If drl is rejecting its pose estimation in your typical use case / environment, then you may need to increase the tolerance to outliers, by specifying to drl that the map data might be X% different in relation to the sensor data.
This is controlled with max_inliers_distance (in meters) and max_outliers_percentage (range 0 to 1 | note that currently this is an outlier_ratio -> 1 equals to 100%)
It is possible to control how far the tracking and recovery algorithms corrections translate and rotate the robot pose with max_transformation_distance | max_transformation_angle and for recovery recovery_2d.yaml#L27 | recovery_2d.yaml#L26
For the initial pose algorithms, currently it not applied this jump limit to allow the robot to start lost and then find itself in the map.
To ensure that ICP runs fast, confirm that you have the look up table enabled with the correspondence_estimation_approach set to CorrespondenceEstimationLookupTable and with its map_cell_resolution equal to your map, otherwise the icp might stop its convergence after reaching its time limit defined in convergence_time_limit_seconds
If your robot is moving very fast, you may need to increase max_correspondence_distance
Have a nice day :)
hello @carlosmccosta my odom is accurate，in odom topic I set 0 to angular_velocity and oritation in X Y axis,and set ignore_height_corrections: true, and it doesnt have roll pitch and Zaxis drit,but it still have x y pose drift and yaw drift,you can see the video below,with the increasement of running time ,the drift also increase,and the drift can be clear by send a initialpose topic ,seem the initialpose will triggle another program that recalculate the pose in the map and clear some drift,what I did is send initialpose every 60s ,but I think it can be fix in code which I can not found it,by the way I disabled the pose_estimation module,so maybe the initpose triggled the pose recovery module and cleared the drift? the tf tree is map->odom->base_link->laser https://drive.google.com/file/d/18HqVuU20vCfN-7rdiI9YQJneGD024vOo/view?usp=share_link this video been record after the robot running 1 hours the laser scan which is base on tf or pose topic should match the point clould ,but it didnt,seems the point clould topic have the correct pose with no drift, but the pose from tf or /localization_detail topic have drift,and this drift can be clear by initalpose
Hello,
When you send an initial pose, the matching algorithm changes from yaml_configuration_tracking_filename to yaml_configuration_recovery_filename
Assuming you have not changed the yamls loaded, you can compare the configurations here:
You should not resend initial poses to fix this issue. Instead, you should adjust the configuration of drl.
Namely, try to increase these 3 parameters:
You can also try and change the tracking algorithm from icp 2d to the standard icp, by commenting this line and uncommenting this line and then changing transformation_estimation_approach to TransformationEstimationSVD
Lastly, if it still has drift, you can disable the look up tables and use the standard k-d trees for correspondence estimation (they are slower, but you can try then and see if the problem persists). For that, change the correspondence_estimation_approach to CorrespondenceEstimation
Have a nice day,
CorrespondenceEstimation
hello @carlosmccosta I have copy [cluttered_environments_dynamic_large_map_2d.yaml] and [recovery_2d.yaml] to the drl and change the [correspondence_estimation_approach] in cluttered_environments_dynamic_large_map_2d.yaml to CorrespondenceEstimation,as you can see the link below,the problem still exist,I think this problem is not about the parameter,the point clould topic /dynamic_robot_localization/ambient_pointcloud and laserscan topic still drift slowly to map seems the drift are connect with the running distance,but /dynamic_robot_localization/aligned_pointcloud always aligned with map.maybe some TF code didn`t wrote properly?
https://drive.google.com/file/d/1AA0dVNP_Xgvrz4QMpX9hlSJ_GixUyJhv/view?usp=share_link
CorrespondenceEstimation
@carlosmccosta I found out that the /dynamic_robot_localization/aligned_pointcloud frame id is map,however the /dynamic_robot_localization/ambient_pointcloud frame id is odom,but the /dynamic_robot_localization/localization_detailed frame is map,
CorrespondenceEstimation
@carlosmccosta I found out that the /dynamic_robot_localization/aligned_pointcloud frame id is map,however the /dynamic_robot_localization/ambient_pointcloud frame id is odom,but the /dynamic_robot_localization/localization_detailed frame is map,
@carlosmccosta looks like the accumulation error of odom has been add to loc pose,I set "assemble_lasers_on_map_frame" default="true" ，the drift problem seems to be fixed, but it came out a new problem,when I send initpose topic,the pose seems very unstable,and is very hard to let laser aligned with map at startup. and the pose is not stable as before when the robot is static. https://drive.google.com/file/d/1Bf7CYHju0hmNpTgOpUr5S3t4NAqcuFPK/view?usp=share_link
Hello,
The ambient_pointcloud topic should be assembled in the odom frame and not in the map frame, because laser -> odom provides a smooth tf chain, while laser -> map has discrete small jumps that are necessary and introduced to correct the odom drift over time. These jumps distort the point cloud assebly when the spherical interpolation is enabled. As such, the point cloud assemby should not be done on the map frame, otherwise, these distortions will cause major oscilations in the alignment performed by drl.
The aligned_pointcloud is the result of the alignment performed by drl and is on map frame, and as such, is the one you should be analyzing in rviz. You should not evaluate the ambient_pointcloud because that is the input for drl and not its result. And of course, since the ambient_pointcloud is on odom frame, it will show on rviz with the odometry drift. That is working as is supposed to.
These frames names are this way on purpose and follow the standard ros tf for mobile robotics: https://www.ros.org/reps/rep-0105.html https://navigation.ros.org/setup_guides/transformation/setup_transforms.html
Looking at your videos, the aligned_pointcloud is correctly overlapping the map. So drl was working correctly all along ! Keep in mind that every localization system has oscillations (which is different than drift) in its pose estimation due to lidar noise and also due to sliding of the alignment within the walls. Namely, if your map has walls with more than one pixel thickness in the occupancy grid, the alignment algorithms may oscillate. This is normal and if you want to minimize this oscillation, you need to have maps with 1 pixel thickness walls.
For inspecting the results of drl, set your rviz world frame to map and inspect the aligned_pointcloud overlap with the occupancy grid / map.
For checking the drl pose, you have the topics localization_pose and localization_detailed
Alternatively, you can check the pose between map and base_link or base_footprint
In drl_configs.yaml you can check the subscribe / input topics and the publish / output topics of drl
Have a nice day,
Hello,
The
ambient_pointcloudtopic should be assembled in theodomframe and not in themapframe, becauselaser->odomprovides a smooth tf chain, whilelaser->maphas discrete small jumps that are necessary and introduced to correct the odom drift over time. These jumps distort the point cloud assebly when the spherical interpolation is enabled. As such, the point cloud assemby should not be done on themapframe, otherwise, these distortions will cause major oscilations in the alignment performed bydrl.The
aligned_pointcloudis the result of the alignment performed by drl and is onmapframe, and as such, is the one you should be analyzing in rviz. You should not evaluate theambient_pointcloudbecause that is the input for drl and not its result. And of course, since theambient_pointcloudis onodomframe, it will show on rviz with the odometry drift. That is working as is supposed to.These frames names are this way on purpose and follow the standard ros tf for mobile robotics: https://www.ros.org/reps/rep-0105.html https://navigation.ros.org/setup_guides/transformation/setup_transforms.html
Looking at your videos, the aligned_pointcloud is correctly overlapping the map. So drl was working correctly all along ! Keep in mind that every localization system has oscillations (which is different than drift) in its pose estimation due to lidar noise and also due to sliding of the alignment within the walls. Namely, if your map has walls with more than one pixel thickness in the occupancy grid, the alignment algorithms may oscillate. This is normal and if you want to minimize this oscillation, you need to have maps with 1 pixel thickness walls.
For inspecting the results of
drl, set your rviz world frame tomapand inspect thealigned_pointcloudoverlap with the occupancy grid / map. For checking the drl pose, you have the topicslocalization_poseandlocalization_detailedAlternatively, you can check the pose betweenmapandbase_linkorbase_footprintIn drl_configs.yaml you can check the subscribe / input topics and the publish / output topics of
drlHave a nice day,
hello @carlosmccosta I tried all the param you suggest,please watch this video,this is the robot ran 1 hours and stoped,the pose of localization_pose localization_detailed and the map->base_link is x=10.29 y=-15.39,then I send a initalpose at x=10.29 y=-15.39,and the pose of X Y axis has changed over 13cm after initpose to x = 10.42 y=-15.25 which is the correct pose to the real world,the map is 0.02 res,and I sure this is not because of oscillations . https://drive.google.com/file/d/18HqVuU20vCfN-7rdiI9YQJneGD024vOo/view