Quad-SDK
All Classes Functions Variables Pages
global_body_plan.h
1#ifndef GLOBAL_BODY_PLAN_H
2#define GLOBAL_BODY_PLAN_H
3
4#include <quad_utils/ros_utils.h>
5
6#include "global_body_planner/planning_utils.h"
7
8using namespace planning_utils;
9
11
17 public:
23
24 bool operator==(GlobalBodyPlan p1) const {
26 }
27
32 inline bool isEmpty() const { return t_plan_.empty(); }
33
38 inline int getStatus() const { return plan_status_; }
39
44 inline void setStatus(int status) { plan_status_ = status; }
45
50 inline int getSize() const { return t_plan_.size(); }
51
56 inline double getDuration() const { return t_plan_.back(); }
57
62 inline double getLength() const { return length_plan_.back(); }
63
68 inline double getLengthAtIndex(int i) const { return length_plan_.at(i); }
69
74 inline double getTime(int i) const { return t_plan_.at(i); }
75
80 inline FullState getStateFromIndex(int i) const { return body_plan_.at(i); }
81
86 inline int getPrimitiveFromIndex(int i) const {
87 return primitive_id_plan_.at(i);
88 }
89
95 inline double getGoalDistance() const { return goal_distance_; }
96
101 inline void setComputedTimestamp(ros::Time timestamp) {
102 computed_timestamp_ = timestamp;
103 }
104
109 inline ros::Time getComputedTimestamp() const { return computed_timestamp_; }
110
115 inline double setPublishedTimestamp(ros::Time timestamp) {
116 published_timestamp_ = timestamp;
117 }
118
123 inline ros::Time getPublishedTimestamp() const {
125 }
126
130 void clear();
131
136 void invalidate();
137
144 void eraseAfterIndex(int start_index);
145
157 void loadPlanData(int plan_status, FullState& start_state,
158 double dist_to_goal, std::vector<State>& state_sequence,
159 std::vector<Action>& action_sequence, double dt, double t0,
160 const PlannerConfig& planner_config);
161
171 void addStateAndGRFToMsg(double t, int plan_index,
172 const FullState& body_state, const GRF& grf,
173 int primitive_id, quad_msgs::RobotPlan& msg);
174
180 void convertToMsg(quad_msgs::RobotPlan& robot_plan_msg,
181 quad_msgs::RobotPlan& discrete_robot_plan_msg);
182
183 private:
186
189
191 std::vector<double> t_plan_;
192
194 std::vector<FullState> body_plan_;
195
197 std::vector<GRF> grf_plan_;
198
200 std::vector<int> primitive_id_plan_;
201
203 std::vector<double> length_plan_;
204
206 std::vector<State> state_sequence_;
207
209 std::vector<Action> action_sequence_;
210
213
216};
217
218#endif // GLOBAL_BODY_PLAN_H
A class to contain global plan data along with helper functions.
Definition global_body_plan.h:16
double setPublishedTimestamp(ros::Time timestamp)
Set the timestamp at which this plan was published.
Definition global_body_plan.h:115
double getLengthAtIndex(int i) const
Get the length of the plan at a particular index in meters.
Definition global_body_plan.h:68
void setStatus(int status)
Set the status of this plan.
Definition global_body_plan.h:44
GlobalBodyPlan()
Constructor for GlobalBodyPlan.
Definition global_body_plan.cpp:3
double goal_distance_
Distance to goal.
Definition global_body_plan.h:215
void addStateAndGRFToMsg(double t, int plan_index, const FullState &body_state, const GRF &grf, int primitive_id, quad_msgs::RobotPlan &msg)
Load a semgent of interpolated plan data into a plan message.
Definition global_body_plan.cpp:74
void clear()
Reset the plan (clear all data, update data to unsolved)
Definition global_body_plan.cpp:8
double getTime(int i) const
Get the time of the plan at a particular index in seconds.
Definition global_body_plan.h:74
ros::Time getPublishedTimestamp() const
Get the timestamp at which this plan was published.
Definition global_body_plan.h:123
std::vector< int > primitive_id_plan_
Std vector containing the interpolated time data.
Definition global_body_plan.h:200
double getDuration() const
Get the duration of the plan in seconds.
Definition global_body_plan.h:56
ros::Time published_timestamp_
Time stamp for when plan was published (not unique)
Definition global_body_plan.h:188
std::vector< double > t_plan_
Std vector containing the interpolated time data.
Definition global_body_plan.h:191
ros::Time computed_timestamp_
Time stamp for when plan was computed (unique)
Definition global_body_plan.h:185
std::vector< Action > action_sequence_
Sequence of discrete actions in the plan.
Definition global_body_plan.h:209
void convertToMsg(quad_msgs::RobotPlan &robot_plan_msg, quad_msgs::RobotPlan &discrete_robot_plan_msg)
Load the entire plan into a plan message.
Definition global_body_plan.cpp:108
std::vector< State > state_sequence_
Sequence of discrete states in the plan.
Definition global_body_plan.h:206
ros::Time getComputedTimestamp() const
Get the timestamp at which this plan was computed.
Definition global_body_plan.h:109
double getLength() const
Get the length of the plan in meters.
Definition global_body_plan.h:62
std::vector< double > length_plan_
Std vector containing the cumulative path length at each index of the plan.
Definition global_body_plan.h:203
std::vector< GRF > grf_plan_
Std vector containing the interpolated wrench plan.
Definition global_body_plan.h:197
bool isEmpty() const
Check if this plan is empty.
Definition global_body_plan.h:32
int getStatus() const
Get the status of this plan.
Definition global_body_plan.h:38
std::vector< FullState > body_plan_
Std vector containing the interpolated robot body plan.
Definition global_body_plan.h:194
int plan_status_
Plan status.
Definition global_body_plan.h:212
void setComputedTimestamp(ros::Time timestamp)
Set the timestamp at which this plan was computed (must be unique)
Definition global_body_plan.h:101
int getPrimitiveFromIndex(int i) const
Get the motion primitive from the plan.
Definition global_body_plan.h:86
void eraseAfterIndex(int start_index)
Clear the plan after a particular index (used for replanning segments)
Definition global_body_plan.cpp:24
int getSize() const
Get the size of the plan (number of finite elements)
Definition global_body_plan.h:50
void invalidate()
Invalidate the plan so next will be accepted (set length to infinite and status to unsolved)
Definition global_body_plan.cpp:19
FullState getStateFromIndex(int i) const
Get a particular state from the plan.
Definition global_body_plan.h:80
double getGoalDistance() const
Get the distance of the plan end to the goal (retrieves member variable, does not compute)
Definition global_body_plan.h:95
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)
Load the interpolated plan data and metadata.
Definition global_body_plan.cpp:35
Define full state with Eigen data.
Definition planning_utils.h:208
Planner Configuration.
Definition planning_utils.h:36