[Unitree Go2] Issues with the qre_go2 SLAM

General Discussion
1

We are currently experiencing a few issues with the SLAM system and would appreciate your assistance. Please find attached a video and several screenshots illustrating the problems we are encountering.

  1. Localization Issue: We initiated the SLAM with the robot positioned in a corner. As shown in the video, when we rotate the robot by 90 degrees, the localization fails to detect the new pose, continually adjusting the robot to its previous position.
  2. Video Feed: The video feed from the robot is not displaying in RViz.
  3. TF Links: RViz indicates that some links in the TF are missing.

We are following the documentation provided at Quadruped Documentation. We are running qre_go2 on a host PC with ROS2 Foxy on the Unitree GO2.

Your expertise and guidance in resolving these issues would be greatly appreciated. Please let us know how we can proceed to troubleshoot and address these problems.

Thank you for your attention to this matter.

TF missing(1)
TF missing(1)1848×1018 199 KB

2

Dear @hasarinduperera,

When performing SLAM with the native lidar of the GO2. It is recommended to move slowly. The issue of why it doesn’t update is because of

slam_toolbox:
  ros__parameters:
    # Plugin params
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None

    # ROS Parameters
    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: /go2/scan
    mode: mapping #localization

    debug_logging: false
    throttle_scans: 5
    transform_publish_period: 0.02 #if 0 never publishes odometry
    map_update_interval: 1.0
    resolution: 0.05
    max_laser_range: 5.0 #for rastering images
    minimum_time_interval: 0.0
    transform_timeout: 0.1
    tf_buffer_duration: 30.
    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
    enable_interactive_mode: true

    # General Parameters
    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.3
    minimum_travel_heading: 0.3
    scan_buffer_size: 10000
    scan_buffer_maximum_scan_distance: 30.0
    link_match_minimum_response_fine: 0.1  
    link_scan_maximum_distance: 1.5
    loop_search_maximum_distance: 3.0
    do_loop_closing: true 
    loop_match_minimum_chain_size: 10           
    loop_match_maximum_variance_coarse: 3.0  
    loop_match_minimum_response_coarse: 0.35    
    loop_match_minimum_response_fine: 0.45

    # Correlation Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1 

    # Correlation Parameters - Loop Closure Parameters
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

    # Scan Matcher Parameters
    distance_variance_penalty: 0.5      
    angle_variance_penalty: 1.0    

    fine_search_angle_offset: 0.00349     
    coarse_search_angle_offset: 0.349   
    coarse_angle_resolution: 0.0349        
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

the minimum travel distance and heading parameter in the slam.yaml file. The sudden movements do not update the map and the 2D scan matcher takes a few seconds to update. The best thing to make it work without changing it is to move slowly.

Use ros2 run teleop_twist_keyboard teleop_twist_keyboard and reduce both the angular and linear velocities!

One option is to tune the SLAM parameters such as reducing the distance and heading parameters but that causes the noise to go out of control.

Another option is to use LIO SLAM as an alternative to SLAM or any other SLAM tool boxes. In the coming months we will either be adding a new SLAM to replace this one and/or optimizing SLAM toolbox because the 2D map is useful for nav2 stuff such planning zones etc.

3

Also please update the repository, I have switched the scanner to cpp and it should not work faster and more reliably then the previous version

4

Hey @Sohail

We are getting following error when we try to run the updated repository. Could you please check? Thank you.

Screenshot from 2024-06-28 19-01-18(1)
Screenshot from 2024-06-28 19-01-18(1)1853×1022 207 KB

5

Please specify which error? The Image stream that is not coming is because it is using the d435i depth camera, you can switch to the other one for the GO2’s front default camera in the left RViz2 menu. For the map, you have to run slam.

Please clone the qre_go2
foxy-nvidia
branch to the go2’s nvidia PC, remove the build install log folder, and rebuild with colcon build --symlink-install

Incase you have the older version of the go2, also do the following once the repository is built

ros2 run robot_upstart uninstall ros2

The new name of the service is qre-platform. Power-cycle the robot, and do sudo service ros2 status. Nothing should come up if uninstalled correctly.

Then run

ros2 run go2_bringup startup_installer.py

Afterwhich, do sudo service qre-platform status and it should show green status.

6

Hey @Sohail
I’m referring to the issue with map generation in slam. After starting the slam process, there is an issue in map transform “could not transform from [ ] to odom”. Can you please help with this?

8

Hey @Sohail is there any updates on this matter? Thank you!

9

Dear @hasarinduperera ,

I have updated to work with cpp in the repository. There was some bug that the scan was not coming due to the Lidar attaching the timestamp of 30 seconds of delay. Please try this again, the scan should work more reliably then before.

Also have you tried this

10

Dear @Sohail,

We were able to get the SLAM running successfully with your recent update, and we greatly appreciate your support.

Currently, we are attempting to get the navigation running using the map created during the mapping process. However, we are encountering an issue where the robot does not start at the correct point on the map. The map loads, but the robot appears outside the mapped area. When we attempt to correct the robot’s position using the 2D pose estimate, it does not work. Additionally, the terminal running the navigation displays an error.

We are attaching a video demonstrating the issue for your review. Could you please check it and assist us in resolving this problem to get the navigation running smoothly?

Your continued support is greatly appreciated.

11

Dear @hasarinduperera,

When ever you want to perform map navigation, always switch the global frame to map. Then you can give it the the pose estimate.