Costmap_2d plug-in

Costmap_2d plug-ins are inherited from CostmapLayer. The specific relationship is shown in the figure below:

StaticLayer

In StaticLayer, it is mainly through receiving map_server to load the static map according to the map topic published by the server. Therefore, the static map can be changed online in StaticLayer.

ObstacleLayer

ObservationBuffer

ObservationBuffer is a buffer of obstacle observation data. The observed obstacle data will be converted into sensor_msgs::msg::PointCloud2 format and store it in ObservationBuffer.

The historical obstacle data stored in the ObservationBuffer can be cleared according to the time you want to keep. The expected holding time is mainly determined by the variable observation_keep_time_ To decide. If it is set to rclcpp::Duration(0.0s), it means that only the latest is stored every time, and the historical obstacle data will be cleared.

Seeing this, some students may think that since obstacles can be removed based on time, can other conditions be used to remove obstacles? The answer must be yes. This needs to be selected according to the application scenario. For example, the moving distance of the robot is used as the judgment condition. When the position of the robot when observing the data is more than the current position of the robot, the data will be cleared.

ObstacleLayer

Obstacle observation data of various sensors can be loaded in the ObstacleLayer. However, the data type only supports PointCloud2 and LaserScan. The data of LaserScan type will be converted into PointCloud2 type data. Because ObservationBuffer only stores PointCloud2 type data.

There are several level parameters in ObstacleLayer that need attention:

declareParameter("enabled", rclcpp::ParameterValue(true));//Enable this layer

declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));//Clear the area occupied by footprint

declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));//Obstacles higher than the height set by this parameter are ignored

declareParameter("combination_method", rclcpp::ParameterValue(1));//Update cost by 0 - > directly overwriting the old data and 1 - > taking the maximum value before and after

declareParameter("observation_sources", rclcpp::ParameterValue(std::string("")));//Name of observation data

The observation data of each sensor can be independently configured with the following parameters:

declareParameter(source + "." + "topic", rclcpp::ParameterValue(source));

declareParameter(source + "." + "sensor_frame", rclcpp::ParameterValue(std::string("")));

declareParameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0));

declareParameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0));

declareParameter(source + "." + "data_type", rclcpp::ParameterValue(std::string("LaserScan")));

declareParameter(source + "." + "min_obstacle_height", rclcpp::ParameterValue(0.0));

declareParameter(source + "." + "max_obstacle_height", rclcpp::ParameterValue(0.0));

declareParameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false));

declareParameter(source + "." + "marking", rclcpp::ParameterValue(true));

declareParameter(source + "." + "clearing", rclcpp::ParameterValue(false));

declareParameter(source + "." + "obstacle_max_range", rclcpp::ParameterValue(2.5));

declareParameter(source + "." + "obstacle_min_range", rclcpp::ParameterValue(0.0));

declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0));

declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0));

There are several important parameters, which are explained here:

observation_persistence: a parameter that determines the duration of an obstacle.

obstacle_max_range，obstacle_min_range: it determines the distance from the installation position of the sensor. Obstacles in the distance range can be marked on the costmap

raytrace_max_range，raytrace_min_range: it determines the distance from the installation position of the sensor, and the obstacles in the distance range can be removed

tf2 is used here_ ROS:: messagefilter to process obstacle observation data. Mainly because of tf2_ros::MessageFilter can ensure that only the fram and global of the sensor_ When the tf relationship of FRAM is valid, execute the callback function of data. When the obstacle data is superimposed on the costmap layer, it is necessary to convert the obstacle data to the global coordinate system. Therefore, it is necessary to ensure that its tf conversion is effective before data processing.

std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>> filter(
	new tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>(
	*sub, *tf_, global_frame_, 50, rclcpp_node_, 
    tf2::durationFromSec(transform_tolerance)));

filter->registerCallback(
	std::bind(
	&ObstacleLayer::pointCloud2Callback, this, std::placeholders::_1,
	observation_buffers_.back()));

RangeSensorLayer

This is a costmap plug-in layer newly added in Navigation2, which mainly maintains ultrasonic data. Its simulation of ultrasonic data uses a cone to characterize the ultrasonic detection space. When mapped to costmap, it is a triangle. This may be the rule graph closest to the ultrasonic testing space! But in order to visually show the difference between this simulation method and the actual ultrasonic testing, I put two pictures below.

Simulated ultrasonic testing space

Actual ultrasonic testing space

In the RangeSensorLayer layer layer, the grid value in the ultrasonic detection area is updated through the probabilistic model.

double sensor = 0.0;
if (!clear) {
	sensor = sensor_model(r, phi, theta);
}
double prior = to_prob(getCost(x, y));//Get the original occupied probability
double prob_occ = sensor * prior;
double prob_not = (1 - sensor) * (1 - prior);
double new_prob = prob_occ / (prob_occ + prob_not);//Update occupied probability

InflationLayer

The soul operation in the InflationLayer is the update function of cost. The following is a brief overview of the function execution process:

1. Extract obstacle points

  // Start with lethal obstacles: by definition distance is 0.0
  auto & obs_bin = inflation_cells_[0];
  for (int j = min_j; j < max_j; j++) {
    for (int i = min_i; i < max_i; i++) {
      int index = static_cast<int>(master_grid.getIndex(i, j));
      unsigned char cost = master_array[index];
      if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) {
        obs_bin.emplace_back(index, i, j, i, j);
      }
    }
  }

1. Iterate obstacle points and update cost

      // assign the cost associated with the distance from an obstacle to the cell
      unsigned char cost = costLookup(mx, my, sx, sy);
      unsigned char old_cost = master_array[index];
      // In order to avoid artifacts appeared out of boundary areas
      // when some layer is going after inflation_layer,
      // we need to apply inflation_layer only to inside of given bounds
      if (static_cast<int>(mx) >= base_min_i &&
        static_cast<int>(my) >= base_min_j &&
        static_cast<int>(mx) < base_max_i &&
        static_cast<int>(my) < base_max_j)
      {
        if (old_cost == NO_INFORMATION &&
          (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
        {
          master_array[index] = cost;
        } else {
          master_array[index] = std::max(old_cost, cost);
        }
      }

1. Add grid points within the expansion radius to the expansion queue

• First, expand the grid around

      // attempt to put the neighbors of the current cell onto the inflation list
      if (mx > 0) {
        enqueue(index - 1, mx - 1, my, sx, sy);
      }
      if (my > 0) {
        enqueue(index - size_x, mx, my - 1, sx, sy);
      }
      if (mx < size_x - 1) {
        enqueue(index + 1, mx + 1, my, sx, sy);
      }
      if (my < size_y - 1) {
        enqueue(index + size_x, mx, my + 1, sx, sy);
      }

• Select the grid within the expansion radius and add it to the expansion queue

void
InflationLayer::enqueue(
  unsigned int index, unsigned int mx, unsigned int my,
  unsigned int src_x, unsigned int src_y)
{
  if (!seen_[index]) {
    // we compute our distance table one cell further than the
    // inflation radius dictates so we can make the check below
    double distance = distanceLookup(mx, my, src_x, src_y);

    // we only want to put the cell in the list if it is within
    // the inflation radius of the obstacle point
    if (distance > cell_inflation_radius_) {
      return;
    }

    const unsigned int r = cell_inflation_radius_ + 2;

    // push the cell data onto the inflation list and mark
    inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back(
      index, mx, my, src_x, src_y);//Push into the vector of the corresponding expansion layer
  }
}

Including inflation_cells_ The grid is stored in circles.

It should be noted that the InflationLayer does not contain a costmap for storing map data_ 2D layer, its only job is to expand the obstacle information on the previous layer in the combination layer.

For the plug-in configuration of costmap, you need to pay attention to the order of configuration. The loading order of plug-ins in the code is based on the configuration order. "Inflation_layer" is usually placed at the end. Because it will eventually expand the obstacle information of the previous layers together. If you don't want to inflate a plug-in layer, you can put it after "inflation_layer".

plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] 

Costmap_2d Filters

Costmap_ The main function of 2D filters is to delimit different functional attributes for the region. For example, KeepoutFilter can realize the function of virtual wall. It can limit some areas where the machine cannot enter or some areas are not recommended to pass. Another example: SpeedFilter limits the speed of the robot in some areas.

How is it different from CostmapLayer?

1. The updateBounds function called by Filter does nothing and does not update the area where cost changes.

void CostmapFilter::updateBounds(
  double robot_x, double robot_y, double robot_yaw,
  double * /*min_x*/, double * /*min_y*/, double * /*max_x*/, double * /*max_y*/)
{
  if (!enabled_) {
    return;
  }

  latest_pose_.x = robot_x;
  latest_pose_.y = robot_y;
  latest_pose_.theta = robot_yaw;
}

1. Filter may not change the grid value. For example, SpeedFilter can only adjust the speed limit according to the grid value maintained by itself. CostmapLayer often needs to modify the grid value.

Costmap_ The relationship between 2D filters is as follows:

Costmap_ The operation mechanism of 2D filters is shown in the following figure:

CostmapFilterInfoServer is responsible for loading some parameters and publishing them to the filter. MapServer mainly loads map files and publishes them to filter.

You may have found out. Filter uses map files to obtain regional information. These map files can be defined by themselves. For example, if you blacken some parts of the original map file, these black parts will be regarded as restricted areas or virtual walls in KeepoutFilter. If you add colors to areas in the map that are not recommended to go, when loaded into KeepoutFilter, these areas with darker colors but not marked as obstacles will have relatively large cost values. In this way, the route planning will try to bypass these areas. Only when other areas are completely infeasible will paths be planned to these areas.

Here is an example of KeepoutFilter:

SpeedFilter will restrict the maximum speed according to the depth of the marked area. The darker the color, the greater the speed constraint. Conversely, the lighter the color, the smaller the speed constraint. There are two ways to constrain speed. One is through percentage, that is, the speed is constrained to the percentage of the maximum speed. One is absolute speed constraint, that is, directly modify the maximum feasible speed.

SpeedFilter will send the calculated speed constraint. nav2_ After receiving the speed constraint topic, the controller updates the corresponding value to the controller plug-in through the function setSpeedLimit, such as TEB and DWB.

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

In addition, the robot is returned to the official account of "the first flight" to get the commonly used technical data of the robot industry, such as C/C++, Python, Docker, Qt, ROS1/2, etc., which are carefully recommended.