contact_state_publisher.h

#ifndef CONTACT_STATE_PUBLISHER_H
#define CONTACT_STATE_PUBLISHER_H
 
#include <gazebo_msgs/ContactsState.h>
#include <geometry_msgs/TransformStamped.h>
#include <quad_msgs/GRFArray.h>
#include <quad_utils/ros_utils.h>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
 
#include <cmath>
#include <eigen3/Eigen/Eigen>
#define MATH_PI 3.141592
 
 
class ContactStatePublisher {
 public:
  ContactStatePublisher(ros::NodeHandle nh);
  void spin();
 
 private:
  void contactStateCallback(const gazebo_msgs::ContactsState::ConstPtr& msg,
                            const int toe_idx);
 
  void publishContactState();
 
  ros::Subscriber toe0_contact_state_sub;
 
  ros::Subscriber toe1_contact_state_sub;
 
  ros::Subscriber toe2_contact_state_sub;
 
  ros::Subscriber toe3_contact_state_sub;
 
  tf2_ros::Buffer buffer_;
 
  tf2_ros::TransformListener listener_;
 
  ros::Publisher grf_pub_;
 
  ros::NodeHandle nh_;
 
  double update_rate_;
 
  const int num_feet_ = 4;
 
  quad_msgs::GRFArray grf_array_msg_;
 
  bool ready_to_publish_;
};
 
#endif  // CONTACT_STATE_PUBLISHER_H