alb_publisher.h

Go to the documentation of this file.

#ifndef _ALB_PUBLISHER_H
#define _ALB_PUBLISHER_H

// ROS headers
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/Odometry.h"
#include "ros/ros.h"

#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/transform_listener.h"

// Osef headers
#include "tlvParser.h"

// Local header
#include "outsight_alb_driver/Zones.h"

class AlbPublisher {
    public:
        AlbPublisher(ros::NodeHandle &nodeHandle);

        void publish(const Tlv::tlv_s *frame);

    private:
        void initPublishers(ros::NodeHandle &nodeHandle);

        void initTransforms();

        void buildZones();

        void publishTimeReference(const Tlv::tlv_s *frame);

        void publishPointCloud(const Tlv::tlv_s *frame);

        void publishPose(const Tlv::tlv_s *frame);

        void publishTrackedObjects(const Tlv::tlv_s *frame);

        void publishEgomotion(const Tlv::tlv_s *frame);

        void publishOdometry(const Tlv::tlv_s *frame);

        void publishAugmentedCloud(const Tlv::tlv_s *frame);

        void publishZones();

        void updatePose(const Tlv::tlv_s *frame);

        void updateTimeStamp(const Tlv::tlv_s *frame);

    private:
        void broadcastAlbTransform(const geometry_msgs::Pose &pose);

        void broadcastMapTransform(const geometry_msgs::Pose &pose);

    private:
        std::string fixed_frame_id;
        std::string base_frame_id;
        std::string sensor_frame_id;
        std::string map_frame_id;

        geometry_msgs::Pose last_pose;
        geometry_msgs::Pose current_pose;
        geometry_msgs::Pose first_pose;
        nav_msgs::Odometry odom_msg;
        bool pose_found;
        bool get_first_pose = false;
        outsight_alb_driver::Zones zones_msg;

        ros::Time ros_time;
        ros::Time alb_time;
        ros::Time last_timestamp;
        ros::Time current_timestamp;
        bool use_alb_time;

        bool use_colwise_order;

        ros::Publisher time_reference_publisher;
        ros::Publisher point_cloud_publisher;
        ros::Publisher pose_publisher;
        ros::Publisher tracked_objects_publisher;
        ros::Publisher egomotion_publisher;
        ros::Publisher odometry_publisher;
        ros::Publisher augmented_cloud_publisher;
        ros::Publisher zones_publisher;

        tf2::Transform base_to_lidar_tf;
        bool transform_broadcaster;
        bool use_base_frame;
};

#endif // _ALB_PUBLISHER_H