global_body_planner.h

#ifndef GLOBAL_BODY_PLANNER_H
#define GLOBAL_BODY_PLANNER_H
 
#include <nav_msgs/Path.h>
#include <quad_msgs/RobotPlan.h>
#include <quad_msgs/RobotState.h>
#include <quad_utils/ros_utils.h>
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <visualization_msgs/MarkerArray.h>
 
#include <grid_map_core/grid_map_core.hpp>
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <grid_map_ros/grid_map_ros.hpp>
 
#include "global_body_planner/gbpl.h"
#include "global_body_planner/global_body_plan.h"
 
using namespace planning_utils;
 
 
class GlobalBodyPlanner {
 public:
  GlobalBodyPlanner(ros::NodeHandle nh);
 
  bool callPlanner();
 
  void spin();
 
 private:
  void terrainMapCallback(const grid_map_msgs::GridMap::ConstPtr &msg);
 
  void robotStateCallback(const quad_msgs::RobotState::ConstPtr &msg);
 
  void goalStateCallback(const geometry_msgs::PointStamped::ConstPtr &msg);
 
  void triggerReset();
 
  int initPlanner();
 
  void clearPlan();
 
  void addStateAndGRFToMsg(double t, int plan_index, FullState body_state,
                           GRF grf, int primitive_id,
                           quad_msgs::RobotPlan &body_plan_msg);
 
  void publishPlan();
 
  void waitForData();
 
  void getInitialPlan();
 
  void setStartState();
 
  void setGoalState();
 
  void publishCurrentPlan();
 
  ros::Subscriber terrain_map_sub_;
 
  ros::Subscriber robot_state_sub_;
 
  ros::Subscriber goal_state_sub_;
 
  ros::Publisher body_plan_pub_;
 
  ros::Publisher discrete_body_plan_pub_;
 
  ros::Publisher tree_pub_;
 
  std::string terrain_map_topic_;
 
  std::string robot_state_topic_;
 
  ros::NodeHandle nh_;
 
  double update_rate_;
 
  int num_calls_;
 
  double max_planning_time_;
 
  std::string map_frame_;
 
  GlobalBodyPlan newest_plan_;
 
  GlobalBodyPlan current_plan_;
 
  FullState start_state_;
 
  FullState goal_state_;
 
  FullState robot_state_;
 
  geometry_msgs::PointStamped::ConstPtr goal_state_msg_;
 
  double replan_start_time_;
 
  double committed_horizon_;
 
  double pos_error_threshold_;
 
  bool restart_flag_;
 
  std::vector<State> state_sequence_;
 
  std::vector<Action> action_sequence_;
 
  std::vector<double> cost_vector_;
 
  std::vector<double> cost_vector_times_;
 
  std::vector<double> solve_time_info_;
 
  std::vector<int> vertices_generated_info_;
 
  PlannerConfig planner_config_;
 
  double reset_publish_delay_;
 
  bool replanning_allowed_;
 
  double dt_;
 
  int planner_status_;
 
  int start_index_;
 
  static const int RESET = 0;
 
  static const int REFINE = 1;
 
  ros::Time reset_time_;
 
  bool publish_after_reset_delay_;
 
  ros::Time global_plan_timestamp_;
};
 
#endif  // GLOBAL_BODY_PLANNER_H