asymmetric_inflation_layer.hpp

// Copyright (c) 2026, Marc Blöchlinger
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef NAV2_COSTMAP_2D__ASYMMETRIC_INFLATION_LAYER_HPP_
#define NAV2_COSTMAP_2D__ASYMMETRIC_INFLATION_LAYER_HPP_
 
#include <algorithm>
#include <vector>
#include <mutex>
#include <memory>
#include <string>
#include <utility>
#include <unordered_map>
#include <cstdint>
 
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/inflation_layer.hpp"
#include "nav_msgs/msg/path.hpp"
 
namespace nav2_costmap_2d
{
 
struct AsymmetricPathSegment
{
  std::pair<double, double> start;
  std::pair<double, double> end;
};
 
enum class Side : int8_t
{
  Right   = -1,
  Neutral =  0,
  Left    =  1,
};
 
class AsymmetricInflationLayer : public nav2_costmap_2d::InflationLayer
{
public:
  AsymmetricInflationLayer();
  ~AsymmetricInflationLayer() override = default;
 
  void onInitialize() override;
 
  void activate() override;
 
  void deactivate() override;
 
  void updateBounds(
    double robot_x, double robot_y, double robot_yaw, double * min_x,
    double * min_y, double * max_x, double * max_y) override;
 
  void updateCosts(
    nav2_costmap_2d::Costmap2D & master_grid,
    int min_i, int min_j, int max_i, int max_j) override;
 
  void matchSize() override;
 
protected:
  void onFootprintChanged() override;
 
  void globalPathCallback(const nav_msgs::msg::Path::ConstSharedPtr msg);
 
  std::vector<AsymmetricPathSegment> extractLocalPathSegments(
    nav2_costmap_2d::Costmap2D & master_grid);
 
  Side computeObstacleSide(
    double cx, double cy,
    const std::vector<size_t> & candidates,
    const std::vector<AsymmetricPathSegment> & local_path_segments);
 
  std::unordered_map<uint64_t, std::vector<size_t>>
  buildPathSpatialHash(
    const std::vector<AsymmetricPathSegment> & local_path_segments);
 
  MatrixXfRM seedDistanceMap(
    nav2_costmap_2d::Costmap2D & master_grid,
    int roi_min_i, int roi_min_j, int roi_width, int roi_height,
    const std::unordered_map<uint64_t, std::vector<size_t>> & spatial_hash,
    const std::vector<AsymmetricPathSegment> & local_path_segments);
 
  void applyInflation(
    unsigned char * master_array,
    const MatrixXfRM & distance_map,
    int min_i, int min_j, int max_i, int max_j,
    int roi_min_i, int roi_min_j,
    unsigned int size_x);
 
  void computeAsymmetricCaches();
 
  rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(
    const std::vector<rclcpp::Parameter> & parameters);
 
  void updateParametersCallback(
    const std::vector<rclcpp::Parameter> & parameters);
 
  // --- Parameters ---
  double cost_scaling_factor_left_;
  double cost_scaling_factor_right_;
  double goal_distance_threshold_;
  std::string plan_topic_;
  tf2::Duration transform_tolerance_;
 
  // --- State ---
  double current_robot_x_{0.0};
  double current_robot_y_{0.0};
  bool asymmetry_active_{false};
 
  // --- Asymmetric Lookup Table ---
  std::vector<unsigned char> cost_lut_disfavored_;
 
  // --- Path subscription ---
  nav2::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
  nav_msgs::msg::Path::ConstSharedPtr latest_global_path_;
  std::mutex path_mutex_;
};
 
}  // namespace nav2_costmap_2d
 
#endif  // NAV2_COSTMAP_2D__ASYMMETRIC_INFLATION_LAYER_HPP_