camera.h

Go to the documentation of this file.

 
#ifndef MULTISENSE_ROS_CAMERA_H
#define MULTISENSE_ROS_CAMERA_H
 
#include <memory>
#include <mutex>
#include <thread>
 
#include <ros/ros.h>
 
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include <image_geometry/stereo_camera_model.h>
#include <image_transport/image_transport.h>
#include <image_transport/camera_publisher.h>
#include <sensor_msgs/distortion_models.h>
#include <stereo_msgs/DisparityImage.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/static_transform_broadcaster.h>
 
#include <multisense_lib/MultiSenseChannel.hh>
#include <multisense_ros/RawCamData.h>
#include <multisense_ros/camera_utilities.h>
#include <multisense_ros/ground_surface_utilities.h>
 
namespace multisense_ros {
 
class Camera {
public:
    Camera(crl::multisense::Channel* driver,
           const std::string& tf_prefix);
    ~Camera();
 
    void updateConfig(const crl::multisense::image::Config& config);
 
    void monoCallback(const crl::multisense::image::Header& header);
    void rectCallback(const crl::multisense::image::Header& header);
    void depthCallback(const crl::multisense::image::Header& header);
    void pointCloudCallback(const crl::multisense::image::Header& header);
    void rawCamDataCallback(const crl::multisense::image::Header& header);
    void colorImageCallback(const crl::multisense::image::Header& header);
    void disparityImageCallback(const crl::multisense::image::Header& header);
    void jpegImageCallback(const crl::multisense::image::Header& header);
    void histogramCallback(const crl::multisense::image::Header& header);
    void colorizeCallback(const crl::multisense::image::Header& header);
    void groundSurfaceCallback(const crl::multisense::image::Header& header);
    void groundSurfaceSplineCallback(const crl::multisense::ground_surface::Header& header);
 
    void borderClipChanged(const BorderClip& borderClipType, double borderClipValue);
 
    void maxPointCloudRangeChanged(double range);
 
    void extrinsicsChanged(crl::multisense::system::ExternalCalibration extrinsics);
 
    void groundSurfaceSplineDrawParametersChanged(
        const ground_surface_utilities::SplineDrawParameters &spline_draw_params);
 
    void timeSyncChanged(bool ptp_enabled, int32_t ptp_time_offset_sec);
 
private:
    //
    // Node names
 
    static constexpr char LEFT[] = "left";
    static constexpr char RIGHT[] = "right";
    static constexpr char AUX[] = "aux";
    static constexpr char CALIBRATION[] = "calibration";
    static constexpr char GROUND_SURFACE[] = "ground_surface";
 
    //
    // Frames
 
    static constexpr char ORIGIN_FRAME[] = "/origin";
    static constexpr char HEAD_FRAME[] = "/head";
    static constexpr char LEFT_CAMERA_FRAME[] = "/left_camera_frame";
    static constexpr char LEFT_RECTIFIED_FRAME[] = "/left_camera_optical_frame";
    static constexpr char RIGHT_CAMERA_FRAME[] = "/right_camera_frame";
    static constexpr char RIGHT_RECTIFIED_FRAME[] = "/right_camera_optical_frame";
    static constexpr char AUX_CAMERA_FRAME[] = "/aux_camera_frame";
    static constexpr char AUX_RECTIFIED_FRAME[] = "/aux_camera_optical_frame";
 
    //
    // Topic names
 
    static constexpr char DEVICE_INFO_TOPIC[] = "device_info";
    static constexpr char RAW_CAM_CAL_TOPIC[] = "raw_cam_cal";
    static constexpr char RAW_CAM_CONFIG_TOPIC[] = "raw_cam_config";
    static constexpr char RAW_CAM_DATA_TOPIC[] = "raw_cam_data";
    static constexpr char HISTOGRAM_TOPIC[] = "histogram";
    static constexpr char MONO_TOPIC[] = "image_mono";
    static constexpr char RECT_TOPIC[] = "image_rect";
    static constexpr char DISPARITY_TOPIC[] = "disparity";
    static constexpr char DISPARITY_IMAGE_TOPIC[] = "disparity_image";
    static constexpr char DEPTH_TOPIC[] = "depth";
    static constexpr char OPENNI_DEPTH_TOPIC[] = "openni_depth";
    static constexpr char COST_TOPIC[] = "cost";
    static constexpr char COLOR_TOPIC[] = "image_color";
    static constexpr char RECT_COLOR_TOPIC[] = "image_rect_color";
    static constexpr char POINTCLOUD_TOPIC[] = "image_points2";
    static constexpr char COLOR_POINTCLOUD_TOPIC[] = "image_points2_color";
    static constexpr char ORGANIZED_POINTCLOUD_TOPIC[] = "organized_image_points2";
    static constexpr char COLOR_ORGANIZED_POINTCLOUD_TOPIC[] = "organized_image_points2_color";
    static constexpr char MONO_CAMERA_INFO_TOPIC[] = "image_mono/camera_info";
    static constexpr char RECT_CAMERA_INFO_TOPIC[] = "image_rect/camera_info";
    static constexpr char COLOR_CAMERA_INFO_TOPIC[] = "image_color/camera_info";
    static constexpr char RECT_COLOR_CAMERA_INFO_TOPIC[] = "image_rect_color/camera_info";
    static constexpr char DEPTH_CAMERA_INFO_TOPIC[] = "depth/camera_info";
    static constexpr char DISPARITY_CAMERA_INFO_TOPIC[] = "disparity/camera_info";
    static constexpr char COST_CAMERA_INFO_TOPIC[] = "cost/camera_info";
    static constexpr char GROUND_SURFACE_IMAGE_TOPIC[] = "image";
    static constexpr char GROUND_SURFACE_INFO_TOPIC[] = "camera_info";
    static constexpr char GROUND_SURFACE_POINT_SPLINE_TOPIC[] = "spline";
 
 
    //
    // Device stream control
 
    void connectStream(crl::multisense::DataSource enableMask);
    void disconnectStream(crl::multisense::DataSource disableMask);
    void stop();
 
    //
    // Republish camera info messages by publishing the current messages
    // Used whenever the resolution of the camera changes
 
    void publishAllCameraInfo();
 
    //
    // CRL sensor API
 
    crl::multisense::Channel* driver_ = nullptr;
 
    //
    // Driver nodes
 
    ros::NodeHandle device_nh_;
    ros::NodeHandle left_nh_;
    ros::NodeHandle right_nh_;
    ros::NodeHandle aux_nh_;
    ros::NodeHandle calibration_nh_;
    ros::NodeHandle ground_surface_nh_;
 
    //
    // Image transports
 
    image_transport::ImageTransport  left_mono_transport_;
    image_transport::ImageTransport  right_mono_transport_;
    image_transport::ImageTransport  left_rect_transport_;
    image_transport::ImageTransport  right_rect_transport_;
    image_transport::ImageTransport  left_rgb_transport_;
    image_transport::ImageTransport  left_rgb_rect_transport_;
    image_transport::ImageTransport  depth_transport_;
    image_transport::ImageTransport  ni_depth_transport_;
    image_transport::ImageTransport  disparity_left_transport_;
    image_transport::ImageTransport  disparity_right_transport_;
    image_transport::ImageTransport  disparity_cost_transport_;
    image_transport::ImageTransport  aux_mono_transport_;
    image_transport::ImageTransport  aux_rgb_transport_;
    image_transport::ImageTransport  aux_rect_transport_;
    image_transport::ImageTransport  aux_rgb_rect_transport_;
    image_transport::ImageTransport  ground_surface_transport_;
 
    //
    // Data publishers
 
    image_transport::Publisher       left_mono_cam_pub_;
    image_transport::Publisher       right_mono_cam_pub_;
    image_transport::CameraPublisher left_rect_cam_pub_;
    image_transport::CameraPublisher right_rect_cam_pub_;
    image_transport::Publisher       depth_cam_pub_;
    image_transport::Publisher       ni_depth_cam_pub_; // publish depth infomation in the openNI format
    image_transport::Publisher       left_rgb_cam_pub_;
    image_transport::CameraPublisher left_rgb_rect_cam_pub_;
    image_transport::Publisher       aux_rgb_cam_pub_;
    image_transport::Publisher       aux_mono_cam_pub_;
    image_transport::CameraPublisher aux_rect_cam_pub_;
    image_transport::CameraPublisher aux_rgb_rect_cam_pub_;
    image_transport::Publisher       ground_surface_cam_pub_;
 
    ros::Publisher                   left_mono_cam_info_pub_;
    ros::Publisher                   right_mono_cam_info_pub_;
    ros::Publisher                   left_rect_cam_info_pub_;
    ros::Publisher                   right_rect_cam_info_pub_;
    ros::Publisher                   left_disp_cam_info_pub_;
    ros::Publisher                   right_disp_cam_info_pub_;
    ros::Publisher                   left_cost_cam_info_pub_;
    ros::Publisher                   left_rgb_cam_info_pub_;
    ros::Publisher                   left_rgb_rect_cam_info_pub_;
    ros::Publisher                   depth_cam_info_pub_;
    ros::Publisher                   aux_mono_cam_info_pub_;
    ros::Publisher                   aux_rgb_cam_info_pub_;
    ros::Publisher                   aux_rect_cam_info_pub_;
    ros::Publisher                   aux_rgb_rect_cam_info_pub_;
    ros::Publisher                   ground_surface_info_pub_;
 
    ros::Publisher                   luma_point_cloud_pub_;
    ros::Publisher                   color_point_cloud_pub_;
    ros::Publisher                   ground_surface_spline_pub_;
 
    ros::Publisher                   luma_organized_point_cloud_pub_;
    ros::Publisher                   color_organized_point_cloud_pub_;
 
    image_transport::Publisher       left_disparity_pub_;
    image_transport::Publisher       right_disparity_pub_;
    image_transport::Publisher       left_disparity_cost_pub_;
 
    ros::Publisher                   left_stereo_disparity_pub_;
    ros::Publisher                   right_stereo_disparity_pub_;
 
    //
    // Raw data publishers
 
    ros::Publisher raw_cam_data_pub_;
    ros::Publisher raw_cam_config_pub_;
    ros::Publisher raw_cam_cal_pub_;
    ros::Publisher device_info_pub_;
    ros::Publisher histogram_pub_;
 
    //
    // Store outgoing messages for efficiency
 
    sensor_msgs::Image         left_mono_image_;
    sensor_msgs::Image         right_mono_image_;
    sensor_msgs::Image         left_rect_image_;
    sensor_msgs::Image         right_rect_image_;
    sensor_msgs::Image         depth_image_;
    sensor_msgs::Image         ni_depth_image_;
    sensor_msgs::PointCloud2   luma_point_cloud_;
    sensor_msgs::PointCloud2   color_point_cloud_;
    sensor_msgs::PointCloud2   luma_organized_point_cloud_;
    sensor_msgs::PointCloud2   color_organized_point_cloud_;
 
    sensor_msgs::Image         aux_mono_image_;
    sensor_msgs::Image         left_rgb_image_;
    sensor_msgs::Image         aux_rgb_image_;
    sensor_msgs::Image         left_rgb_rect_image_;
    sensor_msgs::Image         aux_rect_image_;
    sensor_msgs::Image         aux_rgb_rect_image_;
 
    sensor_msgs::Image         left_disparity_image_;
    sensor_msgs::Image         left_disparity_cost_image_;
    sensor_msgs::Image         right_disparity_image_;
 
    stereo_msgs::DisparityImage left_stereo_disparity_;
    stereo_msgs::DisparityImage right_stereo_disparity_;
 
    sensor_msgs::Image         ground_surface_image_;
 
    multisense_ros::RawCamData raw_cam_data_;
 
    std::vector<uint8_t> pointcloud_color_buffer_;
    std::vector<uint8_t> pointcloud_rect_color_buffer_;
 
    //
    // Calibration from sensor
 
    crl::multisense::system::VersionInfo version_info_;
    crl::multisense::system::DeviceInfo  device_info_;
    std::vector<crl::multisense::system::DeviceMode> device_modes_;
 
    //
    // Calibration manager
 
    std::shared_ptr<StereoCalibrationManager> stereo_calibration_manager_;
 
    //
    // The frame IDs
 
    const std::string frame_id_origin_;
    const std::string frame_id_head_;
    const std::string frame_id_left_;
    const std::string frame_id_right_;
    const std::string frame_id_aux_;
    const std::string frame_id_rectified_left_;
    const std::string frame_id_rectified_right_;
    const std::string frame_id_rectified_aux_;
 
    tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
 
    //
    // Stream subscriptions
 
    typedef std::map<crl::multisense::DataSource, int32_t> StreamMapType;
    std::mutex stream_lock_;
    StreamMapType stream_map_;
 
    //
    // Max distance from the camera for a point to be considered valid
 
    double pointcloud_max_range_ = 15.0;
 
    //
    // Histogram tracking
 
    int64_t last_frame_id_ = -1;
 
    //
    // The mask used to perform the border clipping of the disparity image
 
    BorderClip border_clip_type_ = BorderClip::NONE;
    double border_clip_value_ = 0.0;
 
    //
    // Parameters for drawing ground surface spline
 
    ground_surface_utilities::SplineDrawParameters spline_draw_params_;
 
    //
    // Storage of images which we use for pointcloud colorizing
 
    std::unordered_map<crl::multisense::DataSource, std::shared_ptr<BufferWrapper<crl::multisense::image::Header>>> image_buffers_;
 
    //
    // Has a left camera
 
    bool has_right_camera_ = false;
 
    //
    // Has a left camera
 
    bool has_left_camera_ = false;
 
    //
    // Has a 3rd aux color camera
 
    bool has_aux_camera_ = false;
 
    //
    // Has color camera streams, either color sensors or a color aux
 
    bool has_color_ = false;
 
    //
    // Can support the ground surface detector
 
    bool can_support_ground_surface_ = false;
 
    //
    // Diagnostics
    diagnostic_updater::Updater diagnostic_updater_;
    void deviceInfoDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
    void deviceStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
    void ptpStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
 
    void diagnosticTimerCallback(const ros::TimerEvent &);
    ros::Timer diagnostic_trigger_;
 
    //
    // Timestamping and timesync settings
    ros::Time convertImageTimestamp(uint32_t time_secs, uint32_t time_microsecs);
 
    std::atomic_bool ptp_time_sync_{false};
    int32_t ptp_time_offset_secs_ = 0;
};
 
}
 
#endif