Cartographer_ros: Generating pgm/yaml map

[rsmallegoor]

Since the output of the 'finish_trajectory' service has changed, I'm unable to generate .pgm/.yaml map files for other nodes (non-Cartographer) to use.

I'm aware of the 'write_state' service which provides a .pbstream file, but is it still possible to write to a .pgm/.yaml file?

[damonkohler]

No, that is not yet supported by the asset writer pipeline. It should be resurrected though as it's useful for integration with other ROS nodes.

[zyyfly2010]

ROS already has map_server can do it.
See http://wiki.ros.org/map_server , and
rosrun map_server map_saver -f mymap

[rsmallegoor]

@damonkohler Thank you, i agree it can be useful. I used the previously integrated .pgm generation for this purpose.

@zyyfly2010 I'm aware of the ROS map_server and map_saver. Unfortunately, it doesn't properly recognize the map message on the /map topic from Cartographer.

The map_saver output only contains free and unknown cells, occupied cells seem to be missing. This is probably because of the fact that other ROS integrated SLAM algorithms provide different values as the map data. For example GMapping only defines -1, 0 or 100 instead of the full range from from -1 to 100 like Cartographer does. Or i am missing something?

[Entropy512]

I had to manually make changes to the map_saver source code in order to save values over the entire range of probabilities instead of just "fully occupied", "not occupied at all", and "unknown".

[nnn112358]

I also have troubled with the same problem.Map_saver will convert it to three values ​​without permission.
I would like you to refer to my modification.

map_saver.cpp before
https://github.com/ros-planning/navigation/blob/kinetic-devel/map_server/src/map_saver.cpp
````
for(unsigned int y = 0; y < map->info.height; y++) {
for(unsigned int x = 0; x < map->info.width; x++) {
unsigned int i = x + (map->info.height - y - 1) * map->info.width;
if (map->data[i] == 0) { //occ [0,0.1)
fputc(254, out);
} else if (map->data[i] == +100) { //occ (0.65,1]
fputc(000, out);
} else { //occ [0.1,0.65]
fputc(205, out);
}
}
}

````

after
````
for(unsigned int y = 0; y < map->info.height; y++) {
for(unsigned int x = 0; x < map->info.width; x++) {
unsigned int i = x + (map->info.height - y - 1) * map->info.width;
int pixel = (double)(100.0-map->data[i]) *2.54;
fputc(pixel, out);
}
}

````

[Entropy512]

Yup, that's pretty much what I did - except additionally I mapped -1 to 128

I also changed https://github.com/ros-planning/navigation/blob/kinetic-devel/map_server/src/map_saver.cpp#L114 to print the same thresholds (0.49 and 0.51) that Cartographer used to use, instead of 0.19 and 0.65

[rsmallegoor]

Pretty much how i temporarily solved it for myself: https://github.com/rsmallegoor/navigation/blob/kinetic-devel/map_server/src/map_saver.cpp#L76#L87

if (map->data[i] >= 0 && map->data[i] <=100) {
  unsigned int value = round((float)(100.0-map->data[i])*2.55);
  if (value == 128) {
    fputc(129, out);
  }
  else {
    fputc(value, out);
  }
}
else {
  fputc(128, out);
}

But I'll probably look into resurrecting the code that previously did this in the asset writer if desired.

[ojura]

If anyone else needs it (and it seems here that you do :-) ), #436 (which removed this) is still revertable on top of the current head without (much) pain and it works fine:
https://github.com/larics/cartographer_ros/tree/restore-assets-writer

Would the maintainers (@damonkohler @wohe) consider the possibility of retaining this functionality? I find the ability to directly generate a high-quality occupancy grid from the optimized trajectory very useful; I can then run some additional optimizations, and regenerate the occupancy grid to see the results.

If I remember correctly, this clashes with your desire to get rid of the scan data in the trajectory nodes, which is needed for building the occupancy grid? Perhaps we could find a middle ground, e.g. make that toggleable - if the user doesn't need the data, it is not kept around for increased performance.

Another idea would be to move this into the occupancy grid node and give it the ability to query the SLAM node for full trajectory data (and let the user choose whether to build the grid by fusing submaps or replaying the trajectory). This seems somewhat aligned with the direction you're taking, but involves lots of moving the data around between the nodes.

[wohe]

To be clear and as @damonkohler pointed out we are absolutely in favor of having this functionality. It was implemented in the wrong place though and moving it to the right place reduces memory requirements per time, i.e. longer SLAM using the same amount of memory.

Is there a requirement that the higher quality occupancy grid is available in real-time? If not, this functionality should be added to the assets writer. If someone can make the case for having this in real-time, we could discuss whether the occupancy grid node is the right place to add it.

[ojura]

To be clear and as @damonkohler pointed out we are absolutely in favor of having this functionality.

Of course. I was specifically referring to keeping the functionality of generating the higher quality occupancy grid in real time. My use case is that I am asynchronously pushing aditional data into the node and re-running optimization. The real-time asset writer functionality enables me to preview how this has affected the final high-quality map (rather than the approximation obtained by fusing the submaps, although that is useful too). I can then push additional data, rerun optimization, preview again, etc.

moving it to the right place reduces memory requirements per time, i.e. longer SLAM using the same amount of memory.

In order to generate the high quality map, you still need to somehow store each trajectory node's point cloud data so you can replay it and do mapping with known poses, right? If you didn't want to store it in memory, you would either have to immediately serialize it, or somehow reference the right scan from the bag (although this is not an option with the online node).

Ooops, I forgot that there was a hangout yesterday, I would love to have discussed this in person :-)

[jihoonl]

We are also currently using a forked version that retains occupancy grid generation to simplify the process and to obtain a better map. Probability grid asset writer doesn't seem to generate a map as good as the old one.

[damonkohler]

@jihoonl, what "probability grid asset writer" are you referring to? AFAIK it doesn't exist.

[jihoonl]

I meant ProbabilityGridPointsProcessor. The output png file didn't seem as good as pgm file generated before.

[damonkohler]

I see. We're still missing pgm/yaml output from the asset writer. However, it seems rationale to start by ensuring that the output will be good quality1

The ProbabilityGridPointsProcessor does not do unwarping. Perhaps that's the issue?

[jihoonl]

Um.. what do you mean unwarping?

[damonkohler]

@jihoonl In the local trajectory builders, we call it scan accumulation: https://github.com/googlecartographer/cartographer/blob/ba6f782949eb7cdb96e43c178c7b6ef83bd57215/cartographer/mapping_2d/local_trajectory_builder.cc#L84

We chunk up the scans and then stitch them back together according to pose extrapolation (i.e. constant velocity, odometry, and IMU).

That said, unwarping is relatively new to 2D SLAM. How long have you seen a quality difference in the results of the asset writer as compared to the old-style occupancy grid generation?

[jihoonl]

I had a quick test with the version without scan accumulation(cartographer 3ef680e7914733a1fd15bff784f2217424cc75b5. And I was still able to observe the quality difference.

Removed walls

No dilusion on human footprint

This is our asset writer configuration

options = {
  tracking_frame = "base_footprint",
  pipeline = {
    {
      action = "min_max_range_filter",
      min_range = 0.0,
      max_range = 8.,
    },
    {
      action = "write_probability_grid",
      -- draw_trajectories = false,
      range_data_inserter = {
        insert_free_space = true,
        hit_probability = 0.55,
        miss_probability = 0.49,
      },
      filename = "output",
      resolution = 0.05,
      -- export_meta_data = true
    }
  }
}
return options

Am I misconfiguring something?

[damonkohler]

@jihoonl, @wohe and I discussed this and think it's likely due to the insertion of scans at interpolated positions. If you add an option to only insert the scan that is nearest to each trajectory node, it should solve your problem.

A more complicated approach would be to use an optimization problem to interpolate between trajectory nodes using IMU and odometry data.

[SirVer]

There are at least three issues discussed in this bug. I'd like to refocus this back to the original issue in the title.

@cschuet and I will pair on a PGM and YAML writing points processor that will live in cartographer_ros and bring back the ability to generate ROS compatible maps from Cartographer runs using the assets_writer.

@ojura This will not solve your online use case, which has nothing to do with PGM and YAML and seems to require a bit more discussion and probably design work. If you feel this is worth investigation and further discussion, could you open a new bug against Cartographer to focus the discussion there?

The quality difference between rViz and the asset writer is pulled out into #521.

[rgreid]

For anyone trying to use the ROS map_saver, it now has command line parameters to adjust the free/occupied thresholds, e.g.:

rosrun map_server map_saver -f gridmap --occ 65 --free 20

This is in the Kinetic devel branch also also.