goal_manager.hpp

// Copyright (c) 2020, Samsung Research America
// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd
// Copyright (c) 2024, Stevedan Ogochukwu Omodolor Omodia
//
// 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. Reserved.
 
#ifndef NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_
#define NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_
 
#include <unordered_set>
#include <vector>
#include <functional>
 
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/node_2d.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/node_lattice.hpp"
#include "nav2_smac_planner/node_basic.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
 
 
namespace nav2_smac_planner
{
 
template<typename NodeT>
class GoalManager
{
public:
  typedef NodeT * NodePtr;
  typedef std::vector<NodePtr> NodeVector;
  typedef std::unordered_set<NodePtr> NodeSet;
  typedef std::vector<GoalState<NodeT>> GoalStateVector;
  typedef typename NodeT::Coordinates Coordinates;
  typedef typename NodeT::CoordinateVector CoordinateVector;
 
  GoalManager()
  : _goals_set(NodeSet()),
    _goals_state(GoalStateVector()),
    _goals_coordinate(CoordinateVector()),
    _ref_goal_coord(Coordinates())
  {
  }
 
  ~GoalManager() = default;
 
  bool goalsIsEmpty()
  {
    return _goals_state.empty();
  }
 
  void addGoal(NodePtr & goal)
  {
    _goals_state.push_back({goal, true});
  }
 
  void clear()
  {
    _goals_set.clear();
    _goals_state.clear();
    _goals_coordinate.clear();
  }
 
  void prepareGoalsForAnalyticExpansion(
    NodeVector & coarse_check_goals, NodeVector & fine_check_goals,
    int coarse_search_resolution)
  {
    for (unsigned int i = 0; i < _goals_state.size(); i++) {
      if (_goals_state[i].is_valid) {
        if (i % coarse_search_resolution == 0) {
          coarse_check_goals.push_back(_goals_state[i].goal);
        } else {
          fine_check_goals.push_back(_goals_state[i].goal);
        }
      }
    }
  }
 
  void removeInvalidGoals(
    const float & tolerance,
    GridCollisionChecker * collision_checker,
    const bool & traverse_unknown)
  {
    // Make sure that there was a  goal clear before this was run
    if (!_goals_set.empty() || !_goals_coordinate.empty()) {
      throw std::runtime_error("Goal set should be cleared before calling "
        "removeinvalidgoals");
    }
    for (unsigned int i = 0; i < _goals_state.size(); i++) {
      if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) ||
        tolerance > 0.001)
      {
        _goals_state[i].is_valid = true;
        _goals_set.insert(_goals_state[i].goal);
        _goals_coordinate.push_back(_goals_state[i].goal->pose);
      } else {
        _goals_state[i].is_valid = false;
      }
    }
  }
 
  inline bool isGoal(const NodePtr & node)
  {
    return _goals_set.find(node) != _goals_set.end();
  }
 
  inline NodeSet & getGoalsSet()
  {
    return _goals_set;
  }
 
  inline GoalStateVector & getGoalsState()
  {
    return _goals_state;
  }
 
  inline CoordinateVector & getGoalsCoordinates()
  {
    return _goals_coordinate;
  }
 
  inline void setRefGoalCoordinates(const Coordinates & coord)
  {
    _ref_goal_coord = coord;
  }
 
  inline bool hasGoalChanged(const Coordinates & coord)
  {
    return _ref_goal_coord != coord;
  }
 
protected:
  NodeSet _goals_set;
  GoalStateVector _goals_state;
  CoordinateVector _goals_coordinate;
  Coordinates _ref_goal_coord;
};
 
}  // namespace nav2_smac_planner
 
#endif  // NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_