rviz_interface.h

#ifndef RVIZ_INTERFACE_H
#define RVIZ_INTERFACE_H
 
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <quad_msgs/FootPlanDiscrete.h>
#include <quad_msgs/FootState.h>
#include <quad_msgs/GRFArray.h>
#include <quad_msgs/MultiFootPlanContinuous.h>
#include <quad_msgs/MultiFootPlanDiscrete.h>
#include <quad_msgs/MultiFootState.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_ros/transform_broadcaster.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
 
 
class RVizInterface {
 public:
  RVizInterface(ros::NodeHandle nh);
 
  void spin();
 
 private:
  void robotPlanCallback(const quad_msgs::RobotPlan::ConstPtr &msg,
                         const int pub_id);
 
  void grfCallback(const quad_msgs::GRFArray::ConstPtr &msg);
 
  void discreteBodyPlanCallback(const quad_msgs::RobotPlan::ConstPtr &msg);
 
  void footPlanDiscreteCallback(
      const quad_msgs::MultiFootPlanDiscrete::ConstPtr &msg);
 
  void footPlanContinuousCallback(
      const quad_msgs::MultiFootPlanContinuous::ConstPtr &msg);
 
  void stateEstimateCallback(const quad_msgs::RobotState::ConstPtr &msg);
 
  void robotStateCallback(const quad_msgs::RobotState::ConstPtr &msg,
                          const int pub_id);
 
  ros::Subscriber global_plan_sub_;
 
  ros::Subscriber local_plan_sub_;
 
  ros::Subscriber grf_sub_;
 
  ros::Subscriber discrete_body_plan_sub_;
 
  ros::Subscriber foot_plan_discrete_sub_;
 
  ros::Subscriber foot_plan_continuous_sub_;
 
  ros::Publisher global_plan_viz_pub_;
 
  ros::Publisher local_plan_viz_pub_;
 
  ros::Publisher current_grf_viz_pub_;
 
  ros::Publisher local_plan_ori_viz_pub_;
 
  ros::Publisher global_plan_grf_viz_pub_;
 
  ros::Publisher local_plan_grf_viz_pub_;
 
  ros::Publisher discrete_body_plan_viz_pub_;
 
  ros::Publisher foot_plan_discrete_viz_pub_;
 
  ros::Publisher state_estimate_trace_pub_;
 
  ros::Publisher ground_truth_state_trace_pub_;
 
  ros::Publisher trajectory_state_trace_pub_;
 
  ros::Publisher foot_0_plan_continuous_viz_pub_;
 
  ros::Publisher foot_1_plan_continuous_viz_pub_;
 
  ros::Publisher foot_2_plan_continuous_viz_pub_;
 
  ros::Publisher foot_3_plan_continuous_viz_pub_;
 
  ros::Publisher estimate_joint_states_viz_pub_;
 
  ros::Publisher ground_truth_joint_states_viz_pub_;
 
  ros::Publisher trajectory_joint_states_viz_pub_;
 
  ros::Subscriber state_estimate_sub_;
 
  ros::Subscriber ground_truth_state_sub_;
 
  ros::Subscriber trajectory_state_sub_;
 
  tf2_ros::TransformBroadcaster estimate_base_tf_br_;
 
  tf2_ros::TransformBroadcaster ground_truth_base_tf_br_;
 
  tf2_ros::TransformBroadcaster trajectory_base_tf_br_;
 
  visualization_msgs::Marker state_estimate_trace_msg_;
 
  visualization_msgs::Marker ground_truth_state_trace_msg_;
 
  visualization_msgs::Marker trajectory_state_trace_msg_;
 
  const double trace_reset_threshold_ = 0.2;
 
  ros::NodeHandle nh_;
 
  double update_rate_;
 
  int orientation_subsample_interval_;
 
  std::string map_frame_;
 
  std::string tf_prefix_;
 
  std::vector<int> front_left_color_;
  std::vector<int> back_left_color_;
  std::vector<int> front_right_color_;
  std::vector<int> back_right_color_;
  std::vector<int> net_grf_color_;
  std::vector<int> individual_grf_color_;
 
  const int ESTIMATE = 0;
  const int GROUND_TRUTH = 1;
  const int TRAJECTORY = 2;
 
  const int GLOBAL = 0;
  const int LOCAL = 1;
 
  const int CONNECT = 0;
  const int LEAP_STANCE = 1;
  const int FLIGHT = 2;
  const int LAND_STANCE = 3;
};
 
#endif  // RVIZ_INTERFACE_H