Fixed the problem of static layer not restoring old map values for footprint

[CihatAltiparmak]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4825
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation of turtlebot
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• I have removed clearing the map_buffer just after The method processMap is run so we can utilize map_buffer to restore the region cleared before.
• I have removed map_received_in_update_bounds_ because seems we really don't need to.
• I have left to lock with mutex inside processMap method because now we are using lock in parts we use processMap

Description of documentation updates required from your changes

We have added some methods to Costmap2d. So we may need to update Costmap2D API documentation.

---

Future work that may be required in bullet points

• We can add some methods to Costmap2d. For example, we can should pass the lambda function setConvexPolygonCost in order to implement dynamically how the cost is set.

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@CihatAltiparmak, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

I didn't carefully review this yet (Saturday afternoon over coffee!), but wouldn't it be possible for us to simply remove the setConvexPolygonCost from the internal static layer grid and apply it to the master grid update (master_grid.setConvexPolygonCost()))?

That would apply it more globally however which might not be wanted in some cases (though I find that generally hard to believe some layers would be set for that and others are not, practically speaking).

Alternatively, you could have setConvexPolygonCost split into 2 stages. Stage 1 is getting the cells that belong to the footprint

  // we assume the polygon is given in the global_frame...
  // we need to transform it to map coordinates
  std::vector<MapLocation> map_polygon;
  for (unsigned int i = 0; i < polygon.size(); ++i) {
    MapLocation loc;
    if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) {
      // ("Polygon lies outside map bounds, so we can't fill it");
      return false;
    }
    map_polygon.push_back(loc);
  }

  std::vector<MapLocation> polygon_cells;

  // get the cells that fill the polygon
  convexFillCells(map_polygon, polygon_cells);

  // NEW: Now return this!

And return those. The second step would be to obtain their values in the loop, store them, and set the new values for FREE. After we do the master grid update, use the originally obtained values to repopulate the static layer. You'd basically just break the setConvexPolygonCost into 3 methods: (1) get the cells that belong to a convex shape, (2) sets its value for a fixed value (for free), and (3) sets its value to using a vector of input of values stored originally (to reset).

I think that's similar to what you did, but I think this would require alot less changes.

[CihatAltiparmak]

Thank you for give a feedback in a light speed.

wouldn't it be possible for us to simply remove the setConvexPolygonCost from the internal static layer grid and apply it to the master grid update (master_grid.setConvexPolygonCost()))?

Of course, I can. I just wondered your idea about whether adding new method would be useful for future. For instance, I wanna use setConvexCost in such a way that I can update with max value (updateWithMax). I got my answer.

That would apply it more globally however which might not be wanted in some cases (though I find that generally hard to believe some layers would be set for that and others are not, practically speaking).

I agree with you. I could not get any chance to take a look at other layers, however I can see where this issue comes from. (#4282 )

[SteveMacenski]

Ah, that is pretty recent. Yeah I think doing the second thing I recommended on splitting up the functions would be the best move here. Then, parameterize if you want to repopulate (see comment in the issue ticket)

[CihatAltiparmak]

Hi @SteveMacenski , If you get a chance, would you take a look at my modifications, I've tried to minimize diffs to ease your work. Btw, I can consider to add the test cases to increase code coverage if you want. Additionaly, I will take a look at the other layers.

[SteveMacenski]

Please revert changes to the workflows of the node, I think it creates a number of regressions. I think you should only need to adjust the updateCosts portion of the implementation. I can see why you did this, wanting to restore from the previous iteration's stored values, but I think you can do this much more easily by just restoring a given iteration's costs at the end of the updateCosts method.

[SteveMacenski]

Make sure to open a docs PR on docs.nav2.org that adds this parameter to the static layer & in the migration guide that this change is made

[CihatAltiparmak]

I have removed this parameter, but if you are willing to add it, it can be updated the Costmap2D object itself of static layer in order to to add such a option. Thus, it can be permanent in the master grid. What do you think?

[SteveMacenski]

Revert

[CihatAltiparmak]

I couldn't find how to revert this. Because, if updateFootprint doesn't run continuously, master_grid cannot be restored. But I will think about that.

[SteveMacenski]

How it is being restored now?

[CihatAltiparmak]

If we don't execute updateFootprint before if checks, it will be updated only when new occupancy map msg is published. This is my concern. In my way, if footprint_clearing_enabled is true, the area occupied by robot's footprint is updated per each updateCost execution. In current nav2 stack, it is hard to see that. If I am wrong, fix me please.

Ignore my comment here. I explained its cause here

[CihatAltiparmak]

Instead of updating only when new map has been published, what about to check how updateFootprint updates the bounds of the costmap_? Thus, we can prevent the potential incompabilities between costmap_ and master_grid.

Pseudo is like that.

void
StaticLayer::updateFootprint(
  double robot_x, double robot_y, double robot_yaw,
  double * min_x, double * min_y,
  double * max_x,
  double * max_y)
{
  if (!footprint_clearing_enabled_) {return;}

  for (const auto & point : transformed_footprint_) {
    unsigned int mx, my;
    if (!worldToMap(point.x, point.y, mx, my)) {
      return;
    }

    touch(point.x, point.y, min_x, min_y, max_x, max_y);
  }

  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
}

[SteveMacenski]

Wouldn't this need to happen at the end of the function, after we update the master map?

That would let everything happen in 1 call of updateCosts sequentially under lock without storing and dealing with map / partial map updates between iterations.

[SteveMacenski]

I think some of htese functions can live in the costmap_2d object like setConvexPolygonCost -- then have setConvexPolygonCost itself use some of these to minimize code duplication

[CihatAltiparmak]

It's really cool idea. I am sory I couldn't get it at the beginning of the conversation. It dramatically decreased the diffs.

[SteveMacenski]

I think what this currently does is clear the footprint on the master grid after update, but that would bypass the set updateWithXYZ() policies that users can set in order to set how they want updates to be performed (i.e. use max value or have it overwrite). This would lose information like having non-zero costs due to sensor measurements, even if removing the static map values, which wouldn't be a solution without introducing a regression.

This was a good thought / nice clean way to do this while minimizing code changes, but I think we need to do what we discussed previously unless there's another way around that issue. Break up setConvexPolygonCost into a few methods inside of costmap2d, use those methods here to obtain the cost values originally in the layer's internal costmap, clear the costmap, update the master grid, then revert the layer's internal costmap.

Maybe: Is there a way to apply the updateWithXYZ() policies for the following block? That would reduce the number of localized grid operations back to the same we do now (just each one has an extra couple of checks, which should be minimal in terms of computational cost)

 if (footprint_clearing_enabled_) {
    master_grid.setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE);
  }

I also don't understand why updateFootprint as a method was updated. I think the footprint must be transformed into the current frame before touch-ing the points to expand the bounds. I also don't think that updateFootprint should be completed if not updating its bounds. Can/should these be reverted?

[CihatAltiparmak]

Firstly, my apologies for long delay due to my exams so on.

I think what this currently does is clear the footprint on the master grid after update, but that would bypass the set updateWithXYZ() policies that users can set in order to set how they want updates to be performed (i.e. use max value or have it overwrite). This would lose information like having non-zero costs due to sensor measurements, even if removing the static map values, which wouldn't be a solution without introducing a regression.

I have a question here. why do we have to update the areas with max value or overwrite after clearing the footprint area? Doesn't footprint_clearing_enabled lose its meaning here? But if you still haven't changed your idea, I will try to fix it in no time.

I also don't understand why updateFootprint as a method was updated. I think the footprint must be transformed into the current frame before touch-ing the points to expand the bounds. I also don't think that updateFootprint should be completed if not updating its bounds. Can/should these be reverted?

I commented on one of your reviews, but I saw this later. You are right. Ignore my comments in that review.

I will figure out all of them.

[SteveMacenski]

No need for apologies, life happens 😉

I have a question here. why do we have to update the areas with max value or overwrite after clearing the footprint area? Doesn't footprint_clearing_enabled lose its meaning here? But if you still haven't changed your idea, I will try to fix it in no time.

There's a setting use_max_ that users set for each layer about how to combine the various costmap layer plugins into a single master costmap that's used by the planning/control algorithms. They include updating with max value (i.e. from the existing master costmap, which may have been partially updated by other layers, is this cost greater than the existing one?) or override value (i.e. this value always takes priority, no matter what is already in the master costmap or inserted from other layers before this one).

The behavior that this PR currently implements is overriding of values. The master costmap is always cleared fully under footprint, rather than having the option to combine using the max value that could already exist in the costmap from another layer or a previous update. Imagine you had a laser scan layer below this one - the measurements from the laser scan would be deleted which represent real data being lost by overriding them. The obstacle layer that processes the laser scan has itself a clear under footprint parameter that would need to be also enabled to deal with that. There could be situations where you might not set clear under footprint for all layers -- so deleting the footprint's costs for the full master grid without consulting its combination policy is important.

The clear under footprint is specific to each layer, not to the master costmap itself. The combination policy that the user selected the layer to run at should be respected :-)

if the combination policy is 'override', then what you have is exactly the same as doing the footprint clearing on the local internal costmap to the layer. However, if the policy is 'use maximum', then its not the same. Before this PR, the cleared values under the footprint in the static layer would contribute 0 cost areas, so that maximum value already in from other layers would be retained. The current implementation would clear the values in the master to 0, overriding other layers which previously it just wouldn't update in those areas.