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