Program Listing for File debug_Planner.hpp

↰ Return to documentation for file (/tmp/ws/src/rmf_traffic/rmf_traffic/include/rmf_traffic/agv/debug/debug_Planner.hpp)

/*
 * Copyright (C) 2020 Open Source Robotics Foundation
 *
 * 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 RMF_TRAFFIC__AGV__DEBUG__DEBUG_PLANNER_HPP
#define RMF_TRAFFIC__AGV__DEBUG__DEBUG_PLANNER_HPP

#include <rmf_traffic/agv/Planner.hpp>

#include <queue>

namespace rmf_traffic {
namespace agv {

//==============================================================================
class Planner::Debug
{
public:

  struct Node;
  using ConstNodePtr = std::shared_ptr<const Node>;

  struct Node
  {
    ConstNodePtr parent;

    std::vector<Route> route_from_parent;

    double remaining_cost_estimate;

    double current_cost;

    rmf_utils::optional<std::size_t> waypoint;

    double orientation;

    agv::Graph::Lane::EventPtr event;

    rmf_utils::optional<std::size_t> start_set_index;

    std::size_t id;

    struct Compare
    {
      bool operator()(const ConstNodePtr& a, const ConstNodePtr& b)
      {
        return b->current_cost + b->remaining_cost_estimate
          < a->current_cost + a->remaining_cost_estimate;
      }
    };

    using SearchQueue =
      std::priority_queue<ConstNodePtr, std::vector<ConstNodePtr>, Compare>;

    using Vector = std::vector<ConstNodePtr>;
  };

  class Progress
  {
  public:

    rmf_utils::optional<Plan> step();

    operator bool() const
    {
      return !queue().empty();
    }

    const Node::SearchQueue& queue() const;

    const Node::Vector& expanded_nodes() const;

    const Node::Vector& terminal_nodes() const;

    class Implementation;
  private:
    Progress();
    rmf_utils::unique_impl_ptr<Implementation> _pimpl;
  };

  Debug(const Planner& planner);

  Progress begin(
    const std::vector<Start>& starts,
    Goal goal,
    Options options) const;

  static std::size_t queue_size(const Planner::Result& result);

  static std::size_t expansion_count(const Planner::Result& result);

  static std::size_t node_count(const Planner::Result& result);

  class Implementation;
private:
  rmf_utils::impl_ptr<Implementation> _pimpl;
};


} // namespace agv
} // namespace rmf_traffic

#endif // RMF_TRAFFIC__AGV__DEBUG__DEBUG_PLANNER_HPP