Modify position in hector_mapping code?

edit

Hi guys,

As the question suggests I am trying to modify the HectorMappingRos.cpp/.h from the hector_mapping package and in particular the position used by the mapping function.

My problem is that when my robot goes through long featureless hallways, hector_mapping does not detect a change in position. So what I'm trying to achieve is using odometry from another sensor when this problem happens. It would be a simple switch: when hector_mapping is saying that the robot isn't moving but the odometry says that it is, then take the position from the odometry data.

I have been going through the code and my guess is that I have to modify line 289 of HectorMappingRos.cpp: "startEstimate = slamProcessor->getLastScanMatchPose();".

startEstimate is a vector with the (x,y) coordinates and the yaw angle. My first attempt was to replace the yaw angle with the angle given by an IMU, but this resulted in very bad mapping. Some features were duplicated with a certain angle offset and one hallway was bent.

Has anyone got an opinion on the matter?

EDIT 1:

I have tried setting the undocumented (as in you won't find it on the official ros hector_mapping page but it is there in the code) "use_map_with_known_poses" to "true". The result is that hector_mapping no longer computes the pose of the robot, so the position remains the initial position overtime and every time the map is updated, the new map is simply put on top of the previous one because the position hasn't changed.

I can't seem to be able to "feed" a position topic to hector_mapping. I do not think that it is presently supported by hector_mapping as I do not see any declaration of a subscriber to a position topic in the code.

EDIT 2:

As suggested I set both "map_with_known_poses" and "use_tf_pose_start_estimate" to true. Then I use a first hector_mapping node to publish the map->odom transform that is then being used by a second hector_mapping node for the mapping. Note that it is only for the 2nd node that the 2 parameters mentioned before should be set to true. Here is the map out of the first node:

And here is the map from the 2nd:

Would anyone know why the 2 maps are not identical?