Program Listing for File Trajectory.hpp

↰ Return to documentation for file (include/rmf_traffic/Trajectory.hpp)

/*
 * Copyright (C) 2019 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__TRAJECTORY_HPP
#define RMF_TRAFFIC__TRAJECTORY_HPP

#include <rmf_traffic/geometry/ConvexShape.hpp>
#include <rmf_traffic/Time.hpp>

#include <rmf_utils/impl_ptr.hpp>

#include <Eigen/Geometry>

#include <memory>
#include <vector>

namespace rmf_traffic {

//==============================================================================
namespace internal {
class TrajectoryIteratorImplementation;
} // namespace internal

//==============================================================================
class Trajectory
{
public:

  class Waypoint
  {
  public:

    Eigen::Vector3d position() const;

    Waypoint& position(Eigen::Vector3d new_position);

    Eigen::Vector3d velocity() const;

    Waypoint& velocity(Eigen::Vector3d new_velocity);

    Time time() const;

    std::size_t index() const;

    Waypoint& change_time(Time new_time);

    void adjust_times(Duration delta_t);

    class Implementation;
  private:

    Waypoint();
    Waypoint(const Waypoint&) = delete;
    Waypoint(Waypoint&&) = default;
    Waypoint& operator=(const Waypoint&) = delete;
    Waypoint& operator=(Waypoint&&) = default;
    friend class Trajectory;
    rmf_utils::impl_ptr<Implementation> _pimpl;
  };

  // These classes allow users to traverse the contents of the Trajectory.
  // The trajectory operates much like a typical C++ container, but only for
  // Trajectory::Waypoint information.
  template<typename> class base_iterator;
  using iterator = base_iterator<Waypoint>;
  using const_iterator = base_iterator<const Waypoint>;

  Trajectory();

  // Copy construction/assignment
  Trajectory(const Trajectory& other);
  Trajectory& operator=(const Trajectory& other);

  Trajectory(Trajectory&&) = default;
  Trajectory& operator=(Trajectory&&) = default;

  struct InsertionResult;

  InsertionResult insert(
    Time time,
    Eigen::Vector3d position,
    Eigen::Vector3d velocity);

  InsertionResult insert(const Waypoint& other);

  // TODO(MXG): Consider an insert() function that accepts a range of iterators
  // from another Trajectory instance.

  iterator find(Time time);

  const_iterator find(Time time) const;

  Waypoint& operator[](std::size_t index);

  const Waypoint& operator[](std::size_t index) const;

  Waypoint& at(std::size_t index);

  const Waypoint& at(std::size_t index) const;

  iterator lower_bound(Time time);

  const_iterator lower_bound(Time time) const;

  std::size_t index_after(Time time) const;

  iterator erase(iterator waypoint);

  iterator erase(iterator first, iterator last);

  iterator begin();

  const_iterator begin() const;

  const_iterator cbegin() const;

  iterator end();

  const_iterator end() const;

  const_iterator cend() const;

  Waypoint& front();

  const Waypoint& front() const;

  Waypoint& back();

  const Waypoint& back() const;

  const Time* start_time() const;

  const Time* finish_time() const;

  Duration duration() const;

  std::size_t size() const;

  bool empty() const;

  class Debug;

private:
  friend class internal::TrajectoryIteratorImplementation;
  class Implementation;
  rmf_utils::unique_impl_ptr<Implementation> _pimpl;

};

//==============================================================================
template<typename W>
class Trajectory::base_iterator
{
public:

  W& operator*() const;

  W* operator->() const;

  base_iterator& operator++();

  base_iterator& operator--();

  base_iterator operator++(int);

  base_iterator operator--(int);

  base_iterator operator+(int offset) const;

  base_iterator operator-(int offset) const;

  // TODO(MXG): Consider the spaceship operator when we can use C++20

  bool operator==(const base_iterator& other) const;

  bool operator!=(const base_iterator& other) const;

  bool operator<(const base_iterator& other) const;

  bool operator>(const base_iterator& other) const;

  bool operator<=(const base_iterator& other) const;

  bool operator>=(const base_iterator& other) const;


  // Allow regular iterator to be cast to const_iterator
  operator const_iterator() const;


  // Allow typical copying and moving
  base_iterator(const base_iterator& other) = default;
  base_iterator(base_iterator&& other) = default;
  base_iterator& operator=(const base_iterator& other) = default;
  base_iterator& operator=(base_iterator&& other) = default;

  // Default constructor. This will leave the iterator uninitialized, so it is
  // UNDEFINED BEHAVIOR to use it without using one of the Trajectory functions
  // (like insert, find, etc) to initialize it first.
  base_iterator();

private:
  friend class Trajectory;
  friend class internal::TrajectoryIteratorImplementation;
  rmf_utils::impl_ptr<internal::TrajectoryIteratorImplementation> _pimpl;
};

extern template class Trajectory::base_iterator<Trajectory::Waypoint>;
extern template class Trajectory::base_iterator<const Trajectory::Waypoint>;

//==============================================================================
struct Trajectory::InsertionResult
{
  iterator it;
  bool inserted;
};

} // namespace rmf_traffic

#endif // RMF_TRAFFIC__TRAJECTORY_HPP