local_planner.h

#ifndef LOCAL_PLANNER_H
#define LOCAL_PLANNER_H
 
#include <gtest/gtest_prod.h>
#include <local_planner/local_footstep_planner.h>
#include <local_planner/local_planner_modes.h>
#include <math.h>
#include <nmpc_controller/nmpc_controller.h>
#include <quad_msgs/GRFArray.h>
#include <quad_msgs/MultiFootPlanDiscrete.h>
#include <quad_msgs/RobotPlan.h>
#include <quad_msgs/RobotState.h>
#include <quad_utils/quad_kd.h>
#include <quad_utils/ros_utils.h>
#include <ros/ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 
class LocalPlanner {
 public:
  LocalPlanner(ros::NodeHandle nh);
 
  void spin();
 
 private:
  FRIEND_TEST(LocalPlannerTest, noInputCase);
 
  void initLocalBodyPlanner();
 
  void initLocalFootstepPlanner();
 
  void terrainMapCallback(const grid_map_msgs::GridMap::ConstPtr &msg);
 
  void robotPlanCallback(const quad_msgs::RobotPlan::ConstPtr &msg);
 
  void robotStateCallback(const quad_msgs::RobotState::ConstPtr &msg);
 
  void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg);
 
  void getReference();
 
  bool computeLocalPlan();
 
  void publishLocalPlan();
 
  std::string robot_name_;
 
  ros::Subscriber terrain_map_sub_;
 
  ros::Subscriber body_plan_sub_;
 
  ros::Subscriber robot_state_sub_;
 
  ros::Subscriber cmd_vel_sub_;
 
  ros::Publisher local_plan_pub_;
 
  ros::Publisher foot_plan_discrete_pub_;
 
  ros::Publisher foot_plan_continuous_pub_;
 
  std::string map_frame_;
 
  ros::NodeHandle nh_;
 
  FastTerrainMap terrain_;
 
  grid_map::GridMap terrain_grid_;
 
  double update_rate_;
 
  std::shared_ptr<NMPCController> local_body_planner_nonlinear_;
 
  std::shared_ptr<LocalFootstepPlanner> local_footstep_planner_;
 
  quad_msgs::RobotPlan::ConstPtr body_plan_msg_;
 
  quad_msgs::RobotState::ConstPtr robot_state_msg_;
 
  quad_msgs::MultiFootState past_footholds_msg_;
 
  ros::Time current_state_timestamp_;
 
  Eigen::VectorXd current_state_;
 
  // Current positions of each foot
  Eigen::VectorXd current_foot_positions_world_;
 
  // Current velocities of each foot
  Eigen::VectorXd current_foot_velocities_world_;
 
  // Current positions of each foot
  Eigen::VectorXd current_foot_positions_body_;
 
  int current_plan_index_;
 
  double dt_;
 
  double compute_time_;
 
  double mean_compute_time_;
 
  const double filter_smoothing_constant_ = 0.5;
 
  int N_;
 
  int N_current_;
 
  const int Nx_ = 12;
 
  const int Nu_ = 13;
 
  const int num_feet_ = 4;
 
  const int num_joints_per_leg_ = 3;
 
  Eigen::MatrixXd body_plan_;
 
  Eigen::MatrixXd ref_body_plan_;
 
  Eigen::VectorXd ref_ground_height_;
 
  Eigen::VectorXi ref_primitive_plan_;
 
  Eigen::MatrixXd grf_plan_;
 
  std::vector<std::vector<bool>> contact_schedule_;
 
  Eigen::MatrixXd foot_positions_world_;
 
  Eigen::MatrixXd foot_velocities_world_;
 
  Eigen::MatrixXd foot_accelerations_world_;
 
  Eigen::MatrixXd foot_positions_body_;
 
  Eigen::MatrixXd foot_plan_discrete_;
 
  std::shared_ptr<quad_utils::QuadKD> quadKD_;
 
  Eigen::VectorXd cmd_vel_;
 
  double cmd_vel_filter_const_;
 
  double cmd_vel_scale_;
 
  double z_des_;
 
  ros::Time last_cmd_vel_msg_time_;
 
  double last_cmd_vel_msg_time_max_;
 
  ros::Time initial_timestamp_;
 
  bool first_plan_;
 
  bool use_twist_input_;
 
  Eigen::Vector3d stand_pose_;
 
  double first_element_duration_;
 
  int plan_index_diff_;
 
  double toe_radius_;
 
  int control_mode_;
 
  double stand_vel_threshold_;
 
  double stand_cmd_vel_threshold_;
 
  double stand_pos_error_threshold_;
};
 
#endif  // LOCAL_PLANNER_H