analytic_expansion.hpp

// Copyright (c) 2021, Samsung Research America
//
// 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__ANALYTIC_EXPANSION_HPP_
#define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
 
#include <ompl/base/ScopedState.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
 
#include <functional>
#include <list>
#include <memory>
#include <string>
#include <vector>
 
#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/types.hpp"
#include "nav2_smac_planner/constants.hpp"
 
namespace nav2_smac_planner
{
 
template<typename NodeT>
class AnalyticExpansion
{
public:
  typedef NodeT * NodePtr;
  typedef std::vector<NodePtr> NodeVector;
  typedef typename NodeT::Coordinates Coordinates;
  typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
  typedef typename NodeT::CoordinateVector CoordinateVector;
 
  struct AnalyticExpansionNode
  {
    AnalyticExpansionNode(
      NodePtr & node_in,
      Coordinates & initial_coords_in,
      Coordinates & proposed_coords_in)
    : node(node_in),
      initial_coords(initial_coords_in),
      proposed_coords(proposed_coords_in)
    {
    }
 
    NodePtr node;
    Coordinates initial_coords;
    Coordinates proposed_coords;
  };
 
  struct AnalyticExpansionNodes
  {
    AnalyticExpansionNodes() = default;
 
    void add(
      NodePtr & node,
      Coordinates & initial_coords,
      Coordinates & proposed_coords)
    {
      nodes.emplace_back(node, initial_coords, proposed_coords);
    }
 
    void setDirectionChanges(int changes)
    {
      direction_changes = changes;
    }
 
    std::vector<AnalyticExpansionNode> nodes;
    int direction_changes{0};
  };
 
  AnalyticExpansion(
    const MotionModel & motion_model,
    const SearchInfo & search_info,
    const bool & traverse_unknown,
    const unsigned int & dim_3_size);
 
  void setCollisionChecker(GridCollisionChecker * collision_checker);
 
  NodePtr tryAnalyticExpansion(
    const NodePtr & current_node,
    const NodeVector & coarse_check_goals,
    const NodeVector & fine_check_goals,
    const CoordinateVector & goals_coords,
    const NodeGetter & getter, int & iterations,
    int & closest_distance);
 
  AnalyticExpansionNodes getAnalyticPath(
    const NodePtr & node, const NodePtr & goal,
    const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space);
 
  float refineAnalyticPath(
    NodePtr & node,
    const NodePtr & goal_node,
    const NodeGetter & getter,
    AnalyticExpansionNodes & analytic_nodes);
 
  NodePtr setAnalyticPath(
    const NodePtr & node, const NodePtr & goal,
    const AnalyticExpansionNodes & expanded_nodes);
 
  int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path);
 
  void cleanNode(const NodePtr & nodes);
 
protected:
  MotionModel _motion_model;
  SearchInfo _search_info;
  bool _traverse_unknown;
  unsigned int _dim_3_size;
  GridCollisionChecker * _collision_checker;
  std::list<std::unique_ptr<NodeT>> _detached_nodes;
};
 
}  // namespace nav2_smac_planner
 
#endif  // NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_