waypoint.hpp

/****************************************************************************
 * Copyright (c) 2018 John A. Dougherty. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name ATLFlight nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * In addition Supplemental Terms apply.  See the SUPPLEMENTAL file.
 ****************************************************************************/

#ifndef SNAV_FCI_WAYPOINT_HPP_
#define SNAV_FCI_WAYPOINT_HPP_

#include <Eigen/Dense>

#include "snav/waypoint.hpp"

#include "api/config/waypoint_config.hpp"
#include "api/state_vector.hpp"

namespace snav_fci
{

class Waypoint : public StateVector
{
public:

  enum class Status
  {
    COMPLETE,
    ENROUTE,
    INACTIVE,
  };

  enum Constraint
  {
    FIX_POS=1,
    FIX_VEL=2,
    FIX_ACC=4,
    FIX_JERK=8,
  };

  Waypoint();

  Waypoint(Eigen::Vector3f position);

  Waypoint(Eigen::Vector3f position, WaypointConfig conf);

  Waypoint(StateVector state);

  Waypoint(StateVector state, WaypointConfig conf);

  Waypoint(const snav_traj_gen::Waypoint& waypoint);

  void set_deriv_constraints(int mask);

  void fix_all();

  void fix_none();

  void set_config(WaypointConfig config);

  inline bool is_configured() const
  {
    return configured_;
  }

  inline WaypointConfig get_config() const
  {
    return config_;
  }

  Eigen::Matrix<bool,4,1> constrained; 
  Status status;

  float time; 
  friend std::ostream& operator<<(std::ostream &strm, const Waypoint &wp);

  snav_traj_gen::Waypoint get_traj_gen_waypoint() const;

  static std::string get_status_string(const Status& status);

private:
  WaypointConfig config_;
  bool configured_;

};

} // namespace snav_fci

#endif // SNAV_FCI_WAYPOINT_HPP_