Load static map

[Jin-W-FS]

In these commits I merely completed the method HectorMappingRos::setStaticMapData and use it to load map(s) from the service /static_map at startup and after receiving a "reload" command.

I have the needs to run a device in a same environment again and again with a same world coordinate system. Make hector_mapping load a static map (perhaps with an inaccurate initial pose) will give it a proper beginning.

I use the ros node map_server#map_saver to save hector_mapping data into map files, and map_server#map_server to offer the /static_map service. If so, map_server's /map and /map_metadata topics should be remapped to some other names to avoid conflict. to be exactly, you need something like this in the launch file:

---- updated on 2017/12/15 ----
It is hard to make hector_mapping use the given initial pose to start estimate right after loading a static map. So in the latest commit 0866395, I modified the command format to make two things done in one command.

[AndyZe]

I've tested this PR and it does do what @Jin-W-FS says. 👍 Working on a PR to make the variable names more clear and do a bit of clean-up.

[ghost]

I'm also very interested in this feature. When could this pull request be merged ?

[SushiStar]

This is really a nice function, hope this can be merged into the main branch.
Thanks to Jin-W-FS, this modification just pulled me out of hell:)

[sprajagopal]

My observation with this PR after testing:
(i) I can map my environment and save the map using
rosrun map_server map_saver -f <map_loc>
(ii) In my hector slam launch file I can serve the map by:
<arg name="map_file" default="<map-file>"/> <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

My problem is that the map gets overwritten as soon as hector slam starts. The robot DOES NOT localize(the robot pose does not change accurately) based on previous map but simply overwrites on top of it with new landmarks.
I plan to explore more in the hector slam code to see if there's a way to ONLY LOCALIZE, and no map building.

[sprajagopal]

1. I changed the map_update_distance_thresh, map_update_angle_thresh values to a high value (~400000). This resulted in the map not being updated at all (which is good).

2. But the localization does not happen at all (assuming that the map was made with a certain start position and the robot is currently not at the point).

3. If I move the robot manually a bit (thereby manually aligning the static map and the point cloud), then the slam "latches on", after which localization continues to happen.

4. The /poseupdate topic is publishing a certain covariance matrix which I was interested in (case 1 when the slam does not localize and case 2 when the slam "latches on"). The covariance element values in case 2 are 2x as compared to case 1.

5. From here, it seems the "covariance matrix" from (4) is actually a hessian.

I'm trying to figure out how to make localization possible even with a very poor initial pose.

[bj-neilson]

Hi @skohlbr ,
Did any progress get made with this? I have a geotiff map of my farm that I'd like to be able to publish as a map topic. Then perhaps rely on robot_localisation to manage a rover's position on the map. Finger's crossed!
Thanks,
Ben

[skohlbr]

@bj-neilson: The load functionality in this PR does work for maps in standard ROS map format (see http://wiki.ros.org/map_server#Map_format). GeoTiff maps aren't meant as a format for saving/loading maps, but mainly for visualization.

[zouyonghao]

Any update for this?

[PhehJingJie]

Does anyone have any updates?

[PhehJingJie]

1. I changed the map_update_distance_thresh, map_update_angle_thresh values to a high value (~400000). This resulted in the map not being updated at all (which is good).
2. But the localization does not happen at all (assuming that the map was made with a certain start position and the robot is currently not at the point).
3. If I move the robot manually a bit (thereby manually aligning the static map and the point cloud), then the slam "latches on", after which localization continues to happen.
4. The /poseupdate topic is publishing a certain covariance matrix which I was interested in (case 1 when the slam does not localize and case 2 when the slam "latches on"). The covariance element values in case 2 are 2x as compared to case 1.
5. From here, it seems the "covariance matrix" from (4) is actually a hessian.

I'm trying to figure out how to make localization possible even with a very poor initial pose.

Hi @sprajagopal Did you manage to get the Robot to localize?

[sprajagopal]

1. I changed the map_update_distance_thresh, map_update_angle_thresh values to a high value (~400000). This resulted in the map not being updated at all (which is good).
2. But the localization does not happen at all (assuming that the map was made with a certain start position and the robot is currently not at the point).
3. If I move the robot manually a bit (thereby manually aligning the static map and the point cloud), then the slam "latches on", after which localization continues to happen.
4. The /poseupdate topic is publishing a certain covariance matrix which I was interested in (case 1 when the slam does not localize and case 2 when the slam "latches on"). The covariance element values in case 2 are 2x as compared to case 1.
5. From here, it seems the "covariance matrix" from (4) is actually a hessian.

I'm trying to figure out how to make localization possible even with a very poor initial pose.

Hi @sprajagopal Did you manage to get the Robot to localize?

No. I realized there's a lot of math I needed to understand before debugging the code effectively.

[thuongnv94]

Thanks for nice implementation. However, I got one trouble. The map is built before I set the initial pose for the robot, so that the new map is overwritten with the static map. How can I solve this problem?

[thehuongrbe]

Hi, i had mymap.yaml from map_server then try load statics map by

but nothing map on Rviz and no error on terminal.help me Thank everyone!

[yani-rl-ai]

@Jin-W-FS i tried your modified code, but the map is overwritten. need some help please

[philipzimmermann]

Hi whats the status of this branch? Are there any news? It would be a really useful feature.

[Jbwasse2]

I got localization and continuous mapping done with this pull request (Noetic).
(1) Merge this PR with the current up-to-date branch, build, and source.
(2) In your launch files add map_server node, something like

<arg name="map_file" default="/path/to/map.yaml"/> 
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

If you need to get a map, install map-server

sudo apt-get install ros-noetic-map-server

After this run your hector-mapping code to build your map, once your mapping is finished run

rosrun map_server map_saver --occ 90 --free 10 -f map

to get your map.
(3) Add the "use_static_map" to the hector_mapping rosnode, mine ended up looking as follows

<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
  <param name="map_resolution" value="0.050" />
  <param name="map_size" value="2048" />
  <param name="update_factor_free" value="Put a Float Value Here from 0 to 1, see docs and below info"/>
  <param name="update_factor_occupied" value="Put a Float Value Here from 0 to 1, see docs and below info"/>
  <param name="scan_subscriber_queue_size" value="5" />
  <param name="scan_topic" value="/scan" />
  <param name="pub_map_odom_transform" value="true"/>
  <param name="use_static_map" value="true"/>
  <param name="map_frame" value="map" />
  <param name="base_frame" value="base_link" />
  <param name="odom_frame" value="odom" />
</node>

(4) If you want to do continuous mapping, the default values are 0.4 and 0.9 for the free and occupied values respectively. However, I wanted faster updates to my map and used the values 0.1 and 0.99999. If you want to do localization and do not want to update the map set the values to 0.5 and 0.5. See the documentation and code for Hector-SLAM to get a better idea for the motivation for these values.
(5) Use AMCL to get an estimate of the starting location, and publish this pose to /initialpose. I've found that AMCL gives a good enough starting estimate and after that hector-slam converges nicely. Alternatively, you can use RVIZ's "2D Pose Estimate" button to have an interactive method for giving the /initialpose.

Hope this helps.