1#ifndef GLOBAL_BODY_PLAN_H
2#define GLOBAL_BODY_PLAN_H
4#include <quad_utils/ros_utils.h>
6#include "global_body_planner/planning_utils.h"
8using namespace planning_utils;
158 double dist_to_goal, std::vector<State>& state_sequence,
159 std::vector<Action>& action_sequence,
double dt,
double t0,
172 const FullState& body_state,
const GRF& grf,
173 int primitive_id, quad_msgs::RobotPlan& msg);
181 quad_msgs::RobotPlan& discrete_robot_plan_msg);
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