Localization on Map/Loop Closure in sim_autocross_track.launch

[zghera]

Current/Incorrect Behavior

https://user-images.githubusercontent.com/39711376/103718929-e378d380-4f95-11eb-806f-ed28c6e52c6f.mp4
This video was recorded after the first successful run of the sim_autocross_track.launch when substituting teb_local_planner's config for move base in place of our own in #11.

As in the video above, the abnormal behavior starts once the kart completes a full lap around the track. More specifically, the issue occurs at the "loop closure" on the simulation: connecting the most recently mapped part of the track to the first segment of the track generated.

At the moment the map of the track is nearly complete, it looks like the mapping module tries to re-adjust the walls it has already (recently) mapped. This adjustment seems to propagate as the robot tries to navigate through the track composed of walls from the 1st pass of mapping and the adjusted map. At some point, the robot will run into the walls from the 1st pass because they span the width of the newly adjusted map.

Target Behavior

Upon the connection of the map edges, recognize the loop closure to use the current map and update it as we do more passes.

Potential Approaches to Fix

• ???

[Reschivon]

I have figured out the cause of this issue. However, it might not be worth our time to solve it.

Situation

There are two localization nodes which currently change the robot's position (which is stored as a TF coordinate).

1. The Hector Mapping node looks at the previous and current laserscans, then decides the displacement between them. It then shifts's the robot's global position by this amount
2. The Laser_scan_matcher node does the same, but it also keeps a cumulative map of every laser scanned obstacle encountered.

Issue

When the robot returns to the starting point, there is a slight drift in its perceived position, so the two ends of the explored track do not align perfectly. Thus laser_scan_matcher does not find a satisfactory solution for the translation of the robot. Instead, it finds a halfway solution for the displacement which results in the slipping behavior we see.

Simple fixes

1. Disable cumulative maps for laser_scan_matcher
2. Remove laser_scan_matcher entirely as Hector Mapping does the same thing
3. Try tune parameters in laser_scan_matcher to have larger tolerance

Why fixing this is a Bad Idea

In investigating this issue, I found some key shortcomings in our current localization method.

1. Laser_scan_matcher does not do true loop closing. When there is misalignment, a true loop closing system will repair previous segments of the map so that the loop closes seamlessly. Laser_scan_matcher keeps on going, accumulating drift over multiple laps.
2. Hector mapping and Laser_scan_matcher are not meant to be reliably deployed in robots and are not designed as such. They lack key features of a localization system. And this is rightfully so; 2D laserscan localization is fundamentally flawed:
3. 2D Laserscan localization cuts away most of the data provided by the 3D lidar scan. It cannot work around pitches and rolls of the robot, resulting in major drift. The sparsity of features in a 2D slice limits its accuracy.

Long Term Solution

I suggest we switch to a more fully featured localization method, such as the position tracking available out-of-the-box from the ZED camera SDK, or one of many 3D lidar odometry libraries.

[zghera]

@Reschivon very nice job on finding that and giving a detailed analysis of the issue!

This is another great example showing that the original design was quite incomplete with large holes. Eventually leading to problems like these that require ugly band-aids or a near-complete re-design.

With that, I totally agree that this is not a long-term solution, and trying to fix this issue will probably be a waste of our time. Instead, we should focus our efforts on designing v2.

[zghera]

@Reschivon I created the node to clear the map created by hector_mapping as we discussed yesterday (see hector-reset-map branch). But lo and behold, when I call rosservice list, the reset_map service is nowhere to be found. The only services found for hector_mapping are /hector_mapping/get_loggers and /hector_mapping/set_logger_level and all nodes have these service.

I looked at the parameters for hector_mapping too and couldn't find anything related to launching these services. I looked into the implementation and it looks like they are creating the services right here so I am not sure why none of them are showing up.

[Reschivon]

@zghera That is indeed strange. I don't think it's likely to be a bug in hector_mapping since it's pretty well-used. It could be due to the way we launch or configure the node (🤔) but I can't think of any solid reason why this could be the case.

Are services available for other nodes? Would it be okay to use a modified version of Hector like we do for stage_mod_tf?

[zghera]

@Reschivon I had the same thoughts as you for each of your points/questions. We could try mucking around with Hector like I did for stage_ros and laser_scan_matcher but I am not sure how time intensive that would be.

And here are the services that I get with rosservice list:
/dynamic_map
/hector_mapping/get_loggers
/hector_mapping/set_logger_level
/laser_scan_matcher_node_odom/get_loggers
/laser_scan_matcher_node_odom/set_logger_level
/move_base/GlobalPlanner/make_plan
/move_base/GlobalPlanner/set_parameters
/move_base/TebLocalPlannerROS/set_parameters
/move_base/clear_costmaps
/move_base/get_loggers
/move_base/global_costmap/inflation_layer/set_parameters
/move_base/global_costmap/obstacle_layer/set_parameters
/move_base/global_costmap/set_parameters
/move_base/global_costmap/static_layer/set_parameters
/move_base/local_costmap/obstacle_layer/set_parameters
/move_base/local_costmap/set_parameters
/move_base/local_costmap/static_layer/set_parameters
/move_base/make_plan
/move_base/set_logger_level
/move_base/set_parameters
/reset_positions
/rosout/get_loggers
/rosout/set_logger_level
/rviz/get_loggers
/rviz/reload_shaders
/rviz/set_logger_level
/slam_mode_goal/get_loggers
/slam_mode_goal/set_logger_level
/stage_ros_mod_tf_node/get_loggers
/stage_ros_mod_tf_node/set_logger_level
/ti_comm_node/get_loggers
/ti_comm_node/set_logger_level

[zghera]

I am going to close this issue as we have split off into a few smaller sub-issues based on the two approaches to fix loop closing based on discussions:

1. Modify hector_mapping to such that it does publish the reset_map service that we can call with the node created in the hector-reset-map branch: Clear hector_mapping Map - Fix Loop Closure Strategy #1 #39.
2. Replace hector_mapping entirely where localization is performed by the ZED camera (#40) and we create the map with our own custom node (#41).