laser.h

Go to the documentation of this file.

#ifndef MULTISENSE_ROS_LASER_H
#define MULTISENSE_ROS_LASER_H

#include <mutex>

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2/transform_datatypes.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <multisense_lib/MultiSenseChannel.hh>

namespace multisense_ros {

class Laser {
public:
    Laser(crl::multisense::Channel* driver,
          const std::string& tf_prefix);
    ~Laser();

    void scanCallback(const crl::multisense::lidar::Header& header);
    void pointCloudCallback(const crl::multisense::lidar::Header& header);

    static const float EXPECTED_RATE;

private:

    //
    // Device stream control

    void subscribe();
    void unsubscribe();
    void stop();

    //
    // Transform boadcasting
    void publishStaticTransforms(const ros::Time& time);
    void publishSpindleTransform(const float spindle_angle, const float velocity, const ros::Time& time);

    tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;

    void defaultTfPublisher(const ros::TimerEvent& event);

    //
    // Query transforms

    tf2::Transform getSpindleTransform(float spindle_angle);

    //
    // Calibration from sensor

    crl::multisense::lidar::Calibration lidar_cal_;

    tf2::Transform motor_to_camera_;
    tf2::Transform laser_to_spindle_;

    //
    // Frames to Publish
    std::string left_camera_optical_;
    std::string motor_;
    std::string spindle_;
    std::string hokuyo_;

    //
    // Scan publishing

    crl::multisense::Channel *driver_;
    ros::Publisher            scan_pub_;
    std::string               frame_id_;

    //
    // Raw data publishing

    ros::Publisher raw_lidar_data_pub_;
    ros::Publisher point_cloud_pub_;
    ros::Publisher raw_lidar_cal_pub_;
    ros::Publisher joint_states_pub_;

    //
    // Keep around for efficiency

    sensor_msgs::LaserScan   laser_msg_;
    sensor_msgs::PointCloud2 point_cloud_;
    sensor_msgs::JointState  joint_states_;

    //
    // Subscriptions

    std::mutex sub_lock_;
    int32_t      subscribers_;

    //
    // Timer used to publish the default laser transforms

    ros::Timer timer_;

    //
    // Spindle angle used when publishing the default transforms

    float spindle_angle_;


    //
    // Track publishing rates

    ros::Time previous_scan_time_;


}; // class

}// namespace


#endif