trajectory_publisher.h

#ifndef TRAJECTORY_PUBLISHER_H
#define TRAJECTORY_PUBLISHER_H
 
#include <quad_msgs/RobotPlan.h>
#include <quad_msgs/RobotState.h>
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <visualization_msgs/MarkerArray.h>
 
#include <eigen3/Eigen/Eigen>
 
#include "quad_utils/function_timer.h"
#include "quad_utils/math_utils.h"
#include "quad_utils/quad_kd.h"
#include "quad_utils/ros_utils.h"
 
 
class TrajectoryPublisher {
 public:
  TrajectoryPublisher(ros::NodeHandle nh);
 
  void spin();
 
 private:
  void importTrajectory();
 
  void robotPlanCallback(const quad_msgs::RobotPlan::ConstPtr& msg);
 
  void publishTrajectoryState();
 
  ros::Subscriber body_plan_sub_;
 
  ros::Publisher trajectory_state_pub_;
 
  ros::NodeHandle nh_;
 
  quad_msgs::RobotPlan body_plan_msg_;
 
  quad_msgs::RobotState::ConstPtr robot_state_msg_;
 
  double update_rate_;
 
  std::string map_frame_;
 
  std::string traj_source_;
 
  std::shared_ptr<quad_utils::QuadKD> quadKD_;
};
 
#endif  // TRAJECTORY_PUBLISHER_H