ros_publisher.h 1.56 KB
Newer Older
wangdawei's avatar
wangdawei committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74
#ifndef ROSPUBLISHER_H
#define ROSPUBLISHER_H

#include "localize_utils/pcl_point_type.h"
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl_conversions/pcl_conversions.h>
#include "localize_utils/synchronizedqueue.h"
#include "pcl/common/transforms.h"
#include "localize_utils/ring_queue.h"

#include "publisher.h"

using ImuT = sensor_msgs::Imu;
using NavSatFixT = sensor_msgs::NavSatFix;
using PathT = nav_msgs::Path;
using CloudT = sensor_msgs::PointCloud2;
using ModelT = visualization_msgs::Marker;
namespace juefx{

class RosPublisher : public Publisher
{
public:
    using Ptr = boost::shared_ptr<RosPublisher>;
    RosPublisher();

    ~RosPublisher();

    static Publisher::Ptr Instance();

    void SetRosHandle(ros::NodeHandle* node_handle);

    void init(const string &out_pose_topic,
              const string &out_cloud_topic);

    void load();

    void PublishPose(const IsometryData &pose);

    void PublishCloud(const PointCloudJ &cloud);
private:

    CloudT::Ptr CloudToRosCloud(const PointCloudJ::ConstPtr &pointcloud);

    void PublishMap();

private:

    ros::NodeHandle* _node_handle;

    uint32_t _msg_queue_size;

    ros::Publisher publisher_clouds_;

    ros::Publisher publisher_odom_;

    ros::Publisher publisher_model_;

    ros::Publisher publisher_map_;

    ModelT::Ptr model_;

    PathT path_;

    double lastTime = -1;
};

} // end of namespace
#endif // ROSPUBLISHER_H