color_laser.cpp

Go to the documentation of this file.

 
#include <functional>
 
#include <multisense_ros/color_laser.h>
#include <multisense_ros/point_cloud_utilities.h>
 
//
// Anonymous namespace for locally scoped symbols
namespace {
 
    const uint32_t laser_cloud_step = 16;
 
} // namespace
 
namespace multisense_ros {
 
ColorLaser::ColorLaser(ros::NodeHandle& nh, const std::string &tf_prefix):
    node_handle_(nh),
    image_channels_(3),
    tf_prefix_(tf_prefix)
{
    //
    // Initialize point cloud structure
 
    color_laser_publisher_ = nh.advertise<sensor_msgs::PointCloud2>("lidar_points2_color",
                                                                   10,
                                                                   std::bind(&ColorLaser::startStreaming, this),
                                                                   std::bind(&ColorLaser::stopStreaming, this));
 
}
 
void ColorLaser::colorImageCallback(
    const sensor_msgs::Image::ConstPtr& message
)
{
    std::lock_guard<std::mutex> lock(data_lock_);
 
    //
    // Images are assumed to be 8 bit.
 
    image_channels_ = message->data.size() / (message->height * message->width);
 
    if (image_channels_ > 3)
    {
        ROS_ERROR("Unsupported number of color channels: %d", image_channels_);
    }
 
    color_image_ = *message;
}
 
void ColorLaser::cameraInfoCallback(
    const sensor_msgs::CameraInfo::ConstPtr& message
)
{
    std::lock_guard<std::mutex> lock(data_lock_);
 
    camera_info_ = *message;
}
 
void ColorLaser::laserPointCloudCallback(
    sensor_msgs::PointCloud2::Ptr message
)
{
    std::lock_guard<std::mutex> lock(data_lock_);
 
    //
    // Make sure the associated camera_info/color image is within 2 seconds
    // of the current point cloud message
 
    if (message->header.stamp - color_image_.header.stamp > ros::Duration(2) ||
        message->header.stamp - camera_info_.header.stamp > ros::Duration(2))
    {
        return;
    }
 
    initializePointcloud<float, uint32_t>(color_laser_pointcloud_,
                                          message->header.stamp,
                                          message->width,
                                          message->height,
                                          true,
                                          message->header.frame_id,
                                          "rgb");
 
    //
    // Here we assume that our the sizeof our intensity field is the same as
    // the sizeof our new rgb color field.
 
    float* colorPointCloudDataP = reinterpret_cast<float*>(&(color_laser_pointcloud_.data[0]));
 
    //
    // Iterate over all the points in the point cloud, Use the camera projection
    // matrix to project each point into the left camera image,
    // colorize the point with the corresponding image point in the left
    // camera. If the point does not project into the camera image
    // do not add it to the point cloud.
 
    float* pointCloudDataP = reinterpret_cast<float*>(&(message->data[0]));
 
    const uint32_t height = message->height;
    const uint32_t width = message->width;
    const uint32_t cloudStep = message->point_step / message->fields.size();
 
    uint32_t validPoints = 0;
    for( uint32_t index = 0 ; index < height * width ; ++index, pointCloudDataP += cloudStep)
    {
        float x = pointCloudDataP[0];
        float y = pointCloudDataP[1];
        float z = pointCloudDataP[2];
 
        //
        // Invalid points from the laser will have a distance of 60m.
        // Since these points have the laser calibration applied to them
        // add in a 2m buffer for filtering out invalid points
 
        if (sqrt(x * x + y * y * z * z) > 58.0)
        {
            continue;
        }
 
        //
        // Compute the (u,v) camera coordinates corresponding to our laser
        // point
 
        //
        // (fx*x + cx*z)/z
        const double u = (camera_info_.P[0] * x + camera_info_.P[2] * z) / z;
        //
        // (fy*y + cy*z)/z
        const double v = (camera_info_.P[5] * y + camera_info_.P[6] * z) / z;
 
        //
        // If our computed (u, v) point projects into the image use its
        // color value and add it to the color pointcloud message
 
        if (u < color_image_.width && v < color_image_.height && u >= 0.0 && v >= 0.0)
        {
 
            colorPointCloudDataP[0] = x;
            colorPointCloudDataP[1] = y;
            colorPointCloudDataP[2] = z;
 
            uint8_t* colorChannelP = reinterpret_cast<uint8_t*>(&colorPointCloudDataP[3]);
 
            //
            // Image data is assumed to be BRG and stored continuously in memory
            // both of which are the case for color images from the MultiSense
 
            uint8_t* imageDataP = &color_image_.data[(image_channels_ * static_cast<size_t>(v) * color_image_.width) +
                                                     (image_channels_ * static_cast<size_t>(u))];
 
            switch(image_channels_)
            {
                case 3:
                    colorChannelP[2] = imageDataP[2];
                    colorChannelP[1] = imageDataP[1];
                    colorChannelP[0] = imageDataP[0];
                    break;
                case 2:
                    colorChannelP[1] = imageDataP[1];
                    colorChannelP[0] = imageDataP[0];
                    break;
                case 1:
                    colorChannelP[0] = imageDataP[0];
                    colorChannelP[2] = imageDataP[0];
                    colorChannelP[1] = imageDataP[0];
                    break;
            }
 
            colorPointCloudDataP += cloudStep;
            ++validPoints;
        }
 
    }
 
    color_laser_pointcloud_.data.resize(validPoints * laser_cloud_step);
    color_laser_pointcloud_.width = validPoints;
    color_laser_pointcloud_.row_step = validPoints * laser_cloud_step;
 
    color_laser_publisher_.publish(color_laser_pointcloud_);
}
 
void ColorLaser::startStreaming()
{
    if (color_image_sub_.getNumPublishers() == 0 &&
        camera_info_sub_.getNumPublishers() == 0 &&
        laser_pointcloud_sub_.getNumPublishers() == 0)
    {
        color_image_sub_ = node_handle_.subscribe("image_rect_color",
                                                  10,
                                                  &multisense_ros::ColorLaser::colorImageCallback,
                                                  this);
 
        camera_info_sub_ = node_handle_.subscribe("camera_info",
                                                  10,
                                                  &multisense_ros::ColorLaser::cameraInfoCallback,
                                                  this);
 
        laser_pointcloud_sub_ = node_handle_.subscribe("lidar_points2",
                                                      10,
                                                      &multisense_ros::ColorLaser::laserPointCloudCallback,
                                                      this);
    }
 
}
 
void ColorLaser::stopStreaming()
{
    if (color_laser_publisher_.getNumSubscribers() == 0)
    {
        color_image_sub_.shutdown();
        camera_info_sub_.shutdown();
        laser_pointcloud_sub_.shutdown();
    }
}
 
} // namespace
 
int main(int argc, char** argv)
{
    try
    {
        ros::init(argc, argv, "color_laser_publisher");
 
        ros::NodeHandle nh;
        ros::NodeHandle nh_private("~");
 
        std::string tf_prefix;
        nh_private.param<std::string>("tf_prefix", tf_prefix, "multisense");
 
        multisense_ros::ColorLaser colorLaserPublisher(nh, tf_prefix);
 
        ros::spin();
    }
    catch(std::exception& e)
    {
        ROS_ERROR("%s", e.what());
        return 1;
    }
 
    return 0;
}