[ROS2] map_ Three modes of loading map files on server

Data type of map

The type of map topic is nav\_msgs::msg::OccupancyGrid. Use the following command to query this type of data structure.

ros2 interface show nav\_msgs/msg/OccupancyGrid

nav\_ Msgs:: MSG:: data structure of occupancygrid:

# This represents a 2-D grid map

std\_msgs/Header heade

        builtin\_interfaces/Time stamp

                int32 sec

                uint32 nanosec

        string frame\_id



# MetaData for the map

MapMetaData info

        builtin\_interfaces/Time map\_load\_time

                int32 sec

                uint32 nanosec

        float32 resolution

        uint32 width

        uint32 height

        geometry\_msgs/Pose origin

                Point position

                        float64 x

                        float64 y

                        float64 z

                Quaternion orientation

                        float64 x 0

                        float64 y 0

                        float64 z 0

                        float64 w 1



# The map data, in row-major order, starting with (0,0).

# Cell (1, 0) will be listed second, representing the next cell in the x direction.

# Cell (0, 1) will be at the index equal to info.width, followed by (1, 1).

# The values inside are application dependent, but frequently,

# 0 represents unoccupied, 1 represents definitely occupied, and

# -1 represents unknown.

int8[] data

Where the data member is used to store each grid value in the map. nav\_msgs::msg::OccupancyGrid stores grid values ranging from 0 to 100. 0 indicates that the grid is not occupied, 100 indicates that the grid is occupied, and 0 to 100 indicates the degree of occupation- 1 indicates unknown area.

The info member variable mainly stores some parameters of the map file. For example: map size, resolution, origin and other information.

Three modes of loading map

map\_ The server function pack supports loading three types of image files: PGM/PNG/BMP. The color brightness value of each pixel in the picture will be converted into NAV\_ The grid value in the type msgs:: MSG:: occupancygrid, which is stored in the data member variable.

There are three ways to load maps:

  • trinary
  • scale
  • raw

trinary is the default loading method.

The loading method of the map is usually configured in the configuration file corresponding to the map file. The contents of the configuration file are as follows:

image: map.pgm

resolution: 0.050000

origin: [-10.000000, -10.000000, 0.000000]

negate: 0

occupied\_thresh: 0.65

free\_thresh: 0.196

Corresponding data structure in Code:

struct LoadParameters

{

  std::string image\_file\_name;

  double resolution{0};

  std::vector<double> origin{0, 0, 0};

  double free\_thresh;

  double occupied\_thresh;

  MapMode mode;

  bool negate;

};

If the mode is not specified in the yaml file, the default trinary will be used. free\_thresh and occupied\_thresh is the threshold to determine whether the grid is occupied.

Each pixel in a map picture may have multiple color channels. For example: RGB. The light and shade value of pixels is obtained by calculating the light and shade value of each color channel. The range of the brightness value of the pixel is 0 ~ 1.0. The following is the implementation of the code:

      auto pixel = img.pixelColor(x, y);



      std::vector<Magick::Quantum> channels = {pixel.redQuantum(), pixel.greenQuantum(),

        pixel.blueQuantum()};

      if (load\_parameters.mode == MapMode::Trinary && img.matte()) {

        // To preserve existing behavior, average in alpha with color channels in Trinary mode.

        // CAREFUL. alpha is inverted from what you might expect. High = transparent, low = opaque

        channels.push\_back(MaxRGB - pixel.alphaQuantum());

      }

      double sum = 0;

      for (auto c : channels) {

        sum += c;

      }

      /// on a scale from 0.0 to 1.0 how bright is the pixel?

      double shade = Magick::ColorGray::scaleQuantumToDouble(sum / channels.size());



      // If negate is true, we consider blacker pixels free, and white

      // pixels occupied. Otherwise, it's vice versa.

      /// on a scale from 0.0 to 1.0, how occupied is the map cell (before thresholding)?

      double occ = (load\_parameters.negate ? shade : 1.0 - shade);

By default, we believe that the darker the color, the greater the probability that the grid will be occupied. The brighter the color, the less likely the grid will be occupied.

But when negate is set to 1, the logic is reversed. The darker the color, the less likely the grid will be occupied. The brighter the color, the greater the probability that the grid will be occupied.

Grid assignment method in trinary mode

The judgment in trinary mode is relatively simple. The main reason is that the brightness value of pixels is less than free\_thresh means that the grid is not occupied, and assign 0 to the grid. If greater than occupied\_thresh thinks it's occupied and assigns 100 to the grid. In free\_thresh and occupied\_ Between thresh, it is considered that the state is unknown, and - 1 is assigned to the grid. The following is the specific code implementation:

case MapMode::Trinary:

    if (load\_parameters.occupied\_thresh < occ) {

    map\_cell = nav2\_util::OCC\_GRID\_OCCUPIED;

    } else if (occ < load\_parameters.free\_thresh) {

    map\_cell = nav2\_util::OCC\_GRID\_FREE;

    } else {

    map\_cell = nav2\_util::OCC\_GRID\_UNKNOWN;

    }

Grid assignment method in scale mode

When the Alpha value of the pixel is not 0, the grid value is set to unknown state. The lightness value of the pixel is less than free\_thresh means that the grid is not occupied, and assign 0 to the grid. If greater than occupied\_thresh thinks it's occupied and assigns 100 to the grid. In free\_thresh and occupied\_thresh is linearly converted to 0 ~ 100.

case MapMode::Scale:

    if (pixel.alphaQuantum() != OpaqueOpacity) {

        map\_cell = nav2\_util::OCC\_GRID\_UNKNOWN;

    } else if (load\_parameters.occupied\_thresh < occ) {

        map\_cell = nav2\_util::OCC\_GRID\_OCCUPIED;

    } else if (occ < load\_parameters.free\_thresh) {

        map\_cell = nav2\_util::OCC\_GRID\_FREE;

    } else {

        map\_cell = std::rint(

            (occ - load\_parameters.free\_thresh) /

            (load\_parameters.occupied\_thresh - load\_parameters.free\_thresh) \* 100.0);

    }

    break;

Grid assignment method in raw mode

Take the pixel's lightness value as a percentage and assign the grid value in the way described in the following code.

case MapMode::Raw: {

    double occ\_percent = std::round(shade \* 255);

    if (nav2\_util::OCC\_GRID\_FREE <= occ\_percent &&

        occ\_percent <= nav2\_util::OCC\_GRID\_OCCUPIED)

    {

        map\_cell = static\_cast<int8\_t>(occ\_percent);

    } else {

        map\_cell = nav2\_util::OCC\_GRID\_UNKNOWN;

    }

        break;

    

reference resources:

https://navigation.ros.org/tutorials/docs/navigation2_with_keepout_filter.html#prepare-filter-mask

___

**If you think it's useful, just praise it**

I'm shoufei, a robot development siege lion who helps everyone * * fill the pit * *.

In addition, the robot "reply to the robot" in the official account of "the first flight * *" is used to get the commonly used technical data of the robot industry, such as C/C++, Python, Docker, Qt, ROS1/2, etc.

Tags: ROS2

Posted by cornick on Mon, 18 Apr 2022 09:11:37 +0930