pfuclt_publisher.h

Go to the documentation of this file.

#ifndef PFUCLT_PUBLISHER_H
#define PFUCLT_PUBLISHER_H

#include <pfuclt_omni_dataset/pfuclt_particles.h>
#include <read_omni_dataset/RobotState.h>
#include <read_omni_dataset/LRMGTData.h>
#include <read_omni_dataset/Estimate.h>

#include <vector>
#include <ros/ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/transform_datatypes.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/PointCloud.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/TransformStamped.h>

namespace pfuclt_omni_dataset {

class PFPublisher : public ParticleFilter {
public:
    struct PublishData {
        float robotHeight;

        PublishData(float robotHeight) : robotHeight(robotHeight) {}
    } pubData;

    void resize_particles(const uint n) {
      // Call base class method
      ParticleFilter::resize_particles(n);

      ROS_INFO("Resizing particle message");

      // Resize particle message
      msg_particles_.particles.resize(n);
      for (uint p = 0; p < n; ++p) {
        msg_particles_.particles[p].particle.resize(nSubParticleSets_);
      }
    }

private:
    ros::Subscriber GT_sub_;
    ros::Publisher estimatePublisher_, particlePublisher_,
            targetEstimatePublisher_, targetGTPublisher_, targetParticlePublisher_;
    std::vector<ros::Publisher> particleStdPublishers_;
    std::vector<ros::Publisher> robotGTPublishers_;
    std::vector<ros::Publisher> robotEstimatePublishers_;
    ros::Publisher targetObservationsPublisher_;

    read_omni_dataset::LRMGTData msg_GT_;
    pfuclt_omni_dataset::particles msg_particles_;
    read_omni_dataset::Estimate msg_estimate_;

    std::vector<tf2_ros::TransformBroadcaster> robotBroadcasters;

    void publishParticles();

    void publishRobotStates();

    void publishTargetState();

    void publishEstimate();

    void publishGTData();

    void publishTargetObservations();

public:
    PFPublisher(struct PFinitData &data,
                struct PublishData publishData);

    ParticleFilter *getPFReference() { return (ParticleFilter *) this; }

    void gtDataCallback(const read_omni_dataset::LRMGTData::ConstPtr &);

    void nextIteration();
};
}

#endif //PFUCLT_PUBLISHER_H