global_body_plan.h

#ifndef GLOBAL_BODY_PLAN_H
#define GLOBAL_BODY_PLAN_H
 
#include <quad_utils/ros_utils.h>
 
#include "global_body_planner/planning_utils.h"
 
using namespace planning_utils;
 
 
class GlobalBodyPlan {
 public:
  GlobalBodyPlan();
 
  bool operator==(GlobalBodyPlan p1) const {
    return (p1.getComputedTimestamp() == computed_timestamp_);
  }
 
  inline bool isEmpty() const { return t_plan_.empty(); }
 
  inline int getStatus() const { return plan_status_; }
 
  inline void setStatus(int status) { plan_status_ = status; }
 
  inline int getSize() const { return t_plan_.size(); }
 
  inline double getDuration() const { return t_plan_.back(); }
 
  inline double getLength() const { return length_plan_.back(); }
 
  inline double getLengthAtIndex(int i) const { return length_plan_.at(i); }
 
  inline double getTime(int i) const { return t_plan_.at(i); }
 
  inline FullState getStateFromIndex(int i) const { return body_plan_.at(i); }
 
  inline int getPrimitiveFromIndex(int i) const {
    return primitive_id_plan_.at(i);
  }
 
  inline double getGoalDistance() const { return goal_distance_; }
 
  inline void setComputedTimestamp(ros::Time timestamp) {
    computed_timestamp_ = timestamp;
  }
 
  inline ros::Time getComputedTimestamp() const { return computed_timestamp_; }
 
  inline double setPublishedTimestamp(ros::Time timestamp) {
    published_timestamp_ = timestamp;
  }
 
  inline ros::Time getPublishedTimestamp() const {
    return published_timestamp_;
  }
 
  void clear();
 
  void invalidate();
 
  void eraseAfterIndex(int start_index);
 
  void loadPlanData(int plan_status, FullState& start_state,
                    double dist_to_goal, std::vector<State>& state_sequence,
                    std::vector<Action>& action_sequence, double dt, double t0,
                    const PlannerConfig& planner_config);
 
  void addStateAndGRFToMsg(double t, int plan_index,
                           const FullState& body_state, const GRF& grf,
                           int primitive_id, quad_msgs::RobotPlan& msg);
 
  void convertToMsg(quad_msgs::RobotPlan& robot_plan_msg,
                    quad_msgs::RobotPlan& discrete_robot_plan_msg);
 
 private:
  ros::Time computed_timestamp_;
 
  ros::Time published_timestamp_;
 
  std::vector<double> t_plan_;
 
  std::vector<FullState> body_plan_;
 
  std::vector<GRF> grf_plan_;
 
  std::vector<int> primitive_id_plan_;
 
  std::vector<double> length_plan_;
 
  std::vector<State> state_sequence_;
 
  std::vector<Action> action_sequence_;
 
  int plan_status_;
 
  double goal_distance_;
};
 
#endif  // GLOBAL_BODY_PLAN_H