color_laser.h
Go to the documentation of this file.
00001 
00034 #ifndef MULTISENSE_ROS_COLOR_LASER_H
00035 #define MULTISENSE_ROS_COLOR_LASER_H
00036 
00037 #include <string>
00038 #include <vector>
00039 
00040 #include <boost/thread.hpp>
00041 
00042 #include <ros/ros.h>
00043 #include <sensor_msgs/PointCloud2.h>
00044 #include <sensor_msgs/Image.h>
00045 #include <sensor_msgs/CameraInfo.h>
00046 
00047 namespace multisense_ros{
00048 
00049 class ColorLaser
00050 {
00051     public:
00052 
00053         ColorLaser(
00054             ros::NodeHandle& nh
00055         );
00056 
00057         ~ColorLaser();
00058 
00059 
00060         //
00061         // Callbacks for subscriptions to ROS topics
00062 
00063         void colorImageCallback(const sensor_msgs::Image::ConstPtr& message);
00064         void laserPointCloudCallback(sensor_msgs::PointCloud2::Ptr message);
00065         void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& message);
00066 
00067     private:
00068 
00069         //
00070         // Callback used to setup subscribers once there is a subscription
00071         // to the lidar_points2_color topic
00072 
00073         void startStreaming();
00074         void stopStreaming();
00075 
00076         //
00077         // Messages for local storage of sensor data
00078 
00079         sensor_msgs::Image       color_image_;
00080         sensor_msgs::CameraInfo  camera_info_;
00081 
00082         sensor_msgs::PointCloud2 color_laser_pointcloud_;
00083 
00084         //
00085         // Publisher for the colorized laser point cloud
00086 
00087         ros::Publisher color_laser_publisher_;
00088 
00089         //
00090         // Subscribers for image, point cloud, and camera info topics
00091 
00092         ros::Subscriber color_image_sub_;
00093         ros::Subscriber laser_pointcloud_sub_;
00094         ros::Subscriber camera_info_sub_;
00095 
00096         //
00097         // Node handle used for publishing/subscribing to topics
00098 
00099         ros::NodeHandle node_handle_;
00100 
00101         //
00102         // Mutex to assure callbacks don't interfere with one another
00103 
00104         boost::mutex data_lock_;
00105 
00106         //
00107         // The number of channels in our color image
00108 
00109         uint8_t image_channels_;
00110 };
00111 
00112 }// namespace
00113 
00114 #endif
00115 


multisense_ros
Author(s):
autogenerated on Mon Oct 9 2017 03:06:27