laser.cpp

Go to the documentation of this file.

#include <arpa/inet.h>

#include <angles/angles.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <multisense_ros/laser.h>
#include <multisense_ros/RawLidarData.h>
#include <multisense_ros/RawLidarCal.h>


using namespace crl::multisense;

namespace { // anonymous

const uint32_t laser_cloud_step = 16;

tf2::Transform makeTransform(float T[4][4])
{
    //
    // Manually create the rotation matrix
    tf2::Matrix3x3 rot{T[0][0],
                       T[0][1],
                       T[0][2],
                       T[1][0],
                       T[1][1],
                       T[1][2],
                       T[2][0],
                       T[2][1],
                       T[2][2]};

    //
    // Maually create the translation vector
    tf2::Vector3 trans{T[0][3], T[1][3], T[2][3]};

    return tf2::Transform{rot, trans};
}



} // anonymous

namespace multisense_ros {

const float Laser::EXPECTED_RATE = 40.0;

namespace { // anonymous

//
// Shims for c-style driver callbacks

void lCB(const lidar::Header&        header,
         void*                       userDataP)
{
    reinterpret_cast<Laser*>(userDataP)->scanCallback(header);
}

void pCB(const lidar::Header&        header,
         void*                       userDataP)
{
    reinterpret_cast<Laser*>(userDataP)->pointCloudCallback(header);
}

} // anonymous

Laser::Laser(Channel* driver,
             const std::string& tf_prefix):
    driver_(driver),
    subscribers_(0),
    spindle_angle_(0.0),
    previous_scan_time_(0.0)

{
    //
    // Get device info

    system::DeviceInfo  deviceInfo;

    Status status = driver_->getDeviceInfo(deviceInfo);
    if (Status_Ok != status) {
        ROS_ERROR("Laser: failed to query device info: %s",
                  Channel::statusString(status));
        return;
    }

    if (deviceInfo.hardwareRevision != system::DeviceInfo::HARDWARE_REV_MULTISENSE_SL) {
        ROS_INFO("hardware does not support a laser");
        return;
    }

    ros::NodeHandle nh("");

    //
    // Set frame ID

    frame_id_ = tf_prefix + "/head_hokuyo_frame";

    left_camera_optical_ =  tf_prefix + "/" + "left_camera_optical_frame";
    motor_               =  tf_prefix + "/" + "motor";
    spindle_             =  tf_prefix + "/" + "spindle";
    hokuyo_              =  tf_prefix + "/" + "hokuyo_link";

    ROS_INFO("laser frame id: %s", frame_id_.c_str());

    //
    // Stop lidar stream

    stop();

    //
    // Query calibration from sensor

    status = driver_->getLidarCalibration(lidar_cal_);
    if (Status_Ok != status)
        ROS_WARN("could not query lidar calibration (%s), using URDF defaults",
                 Channel::statusString(status));
    else {

        //
        // Create two static transforms representing sensor calibration
        motor_to_camera_ = makeTransform(lidar_cal_.cameraToSpindleFixed);
        laser_to_spindle_ = makeTransform(lidar_cal_.laserToSpindle);

    }

    //
    // Create scan publisher

    scan_pub_ = nh.advertise<sensor_msgs::LaserScan>("lidar_scan", 20,
                std::bind(&Laser::subscribe, this),
                std::bind(&Laser::unsubscribe, this));

    //
    // Initialize point cloud structure

    point_cloud_.is_bigendian    = (htonl(1) == 1);
    point_cloud_.is_dense        = true;
    point_cloud_.point_step      = laser_cloud_step;
    point_cloud_.height          = 1;
    point_cloud_.header.frame_id =  tf_prefix + "/left_camera_optical_frame";

    point_cloud_.fields.resize(4);
    point_cloud_.fields[0].name     = "x";
    point_cloud_.fields[0].offset   = 0;
    point_cloud_.fields[0].count    = 1;
    point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
    point_cloud_.fields[1].name     = "y";
    point_cloud_.fields[1].offset   = 4;
    point_cloud_.fields[1].count    = 1;
    point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
    point_cloud_.fields[2].name     = "z";
    point_cloud_.fields[2].offset   = 8;
    point_cloud_.fields[2].count    = 1;
    point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
    point_cloud_.fields[3].name     = "intensity";
    point_cloud_.fields[3].offset   = 12;
    point_cloud_.fields[3].count    = 1;
    point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;

    //
    // Create point cloud publisher

    point_cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("lidar_points2", 5,
                       std::bind(&Laser::subscribe, this),
                       std::bind(&Laser::unsubscribe, this));

    //
    // Create calibration publishers

    ros::NodeHandle calibration_nh(nh, "calibration");
    raw_lidar_cal_pub_  = calibration_nh.advertise<multisense_ros::RawLidarCal>("raw_lidar_cal", 1, true);
    raw_lidar_data_pub_ = calibration_nh.advertise<multisense_ros::RawLidarData>("raw_lidar_data", 20,
                          std::bind(&Laser::subscribe, this),
                          std::bind(&Laser::unsubscribe, this));

    //
    // Publish calibration

    multisense_ros::RawLidarCal ros_msg;

    const float *calP = reinterpret_cast<const float*>(&(lidar_cal_.laserToSpindle[0][0]));
    for(uint32_t i=0; i<16; ++i)
        ros_msg.laserToSpindle[i] = calP[i];

    calP = reinterpret_cast<const float*>(&(lidar_cal_.cameraToSpindleFixed[0][0]));
    for(uint32_t i=0; i<16; ++i)
        ros_msg.cameraToSpindleFixed[i] = calP[i];

    raw_lidar_cal_pub_.publish(ros_msg);


    //
    // Populate the jointstates message for publishing the laser spindle
    // angle

    joint_states_.name.resize(1);
    joint_states_.position.resize(1);
    joint_states_.velocity.resize(1);
    joint_states_.effort.resize(1);
    joint_states_.name[0] = tf_prefix + "/motor_joint";
    joint_states_.position[0] = 0.0;
    joint_states_.velocity[0] = 0.0;
    joint_states_.effort[0] = 0.0;

    //
    // Create a joint state publisher

    joint_states_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states", 1, true);

    //
    // Publish the static transforms from our calibration.

    std::vector<geometry_msgs::TransformStamped> stamped_transforms(2);
    stamped_transforms[0].header.stamp = ros::Time::now();
    stamped_transforms[0].header.frame_id = left_camera_optical_;
    stamped_transforms[0].child_frame_id = motor_;
    stamped_transforms[0].transform = tf2::toMsg(motor_to_camera_);

    stamped_transforms[1].header.stamp = ros::Time::now();
    stamped_transforms[1].header.frame_id = spindle_;
    stamped_transforms[1].child_frame_id = hokuyo_;
    stamped_transforms[1].transform = tf2::toMsg(laser_to_spindle_);

    static_tf_broadcaster_.sendTransform(stamped_transforms);

    //
    // Create a timer routine to publish the laser transform even when nothing
    // is subscribed to the laser topics. Publishing occurs at 1Hz

    timer_ = nh.createTimer(ros::Duration(1), &Laser::defaultTfPublisher, this);

    //
    // Register callbacks, driver creates dedicated background thread for each

    driver_->addIsolatedCallback(lCB, this);
    driver_->addIsolatedCallback(pCB, this);


}

Laser::~Laser()
{
    std::lock_guard<std::mutex> lock(sub_lock_);
    stop();
    driver_->removeIsolatedCallback(lCB);
    driver_->removeIsolatedCallback(pCB);
}


void Laser::pointCloudCallback(const lidar::Header& header)
{
    //
    // Get out if we have no work to do

    if (0 == point_cloud_pub_.getNumSubscribers())
        return;

    point_cloud_.data.resize(laser_cloud_step * header.pointCount);
    point_cloud_.row_step     = header.pointCount * laser_cloud_step;
    point_cloud_.width        = header.pointCount;
    point_cloud_.header.stamp = ros::Time(header.timeStartSeconds,
                                          1000 * header.timeStartMicroSeconds);
    //
    // For convenience below

    uint8_t       *cloudP            = reinterpret_cast<uint8_t*>(&point_cloud_.data[0]);
    const uint32_t pointSize         = 3 * sizeof(float); // x, y, z
    const double   arcRadians        = 1e-6 * static_cast<double>(header.scanArc);
    const double   mirrorThetaStart  = -arcRadians / 2.0;
    const double   spindleAngleStart = angles::normalize_angle(1e-6 * static_cast<double>(header.spindleAngleStart));
    const double   spindleAngleEnd   = angles::normalize_angle(1e-6 * static_cast<double>(header.spindleAngleEnd));
    const double   spindleAngleRange = angles::normalize_angle(spindleAngleEnd - spindleAngleStart);

    for(uint32_t i=0; i<header.pointCount; ++i, cloudP += laser_cloud_step) {

        //
        // Percent through the scan arc

        const double percent = static_cast<double>(i) / static_cast<double>(header.pointCount - 1);

        //
        // The mirror angle for this point

        const double mirrorTheta = mirrorThetaStart + percent * arcRadians;

        //
        // The rotation about the spindle

        const double spindleTheta = spindleAngleStart + percent * spindleAngleRange;

        const tf2::Transform spindle_to_motor = getSpindleTransform(spindleTheta);

        //
        // The coordinate in left optical frame

        const double      rangeMeters = 1e-3 * static_cast<double>(header.rangesP[i]);  // from millimeters
        const tf2::Vector3 pointMotor  = (laser_to_spindle_ *
                                         tf2::Vector3(rangeMeters * std::sin(mirrorTheta), 0.0,
                                                      rangeMeters *  std::cos(mirrorTheta)));
        const tf2::Vector3 pointCamera = motor_to_camera_ * (spindle_to_motor * pointMotor);

        //
        // Copy data to point cloud structure

        const float xyz[3] = {static_cast<float>(pointCamera.getX()),
                              static_cast<float>(pointCamera.getY()),
                              static_cast<float>(pointCamera.getZ())};

        memcpy(cloudP, &(xyz[0]), pointSize);
        float* intensityChannel = reinterpret_cast<float*>(cloudP + pointSize);
        *intensityChannel = static_cast<float>(header.intensitiesP[i]);   // in device units
    }

    point_cloud_pub_.publish(point_cloud_);
}

void Laser::scanCallback(const lidar::Header& header)
{

    const ros::Time start_absolute_time = ros::Time(header.timeStartSeconds,
                                                    1000 * header.timeStartMicroSeconds);
    const ros::Time end_absolute_time   = ros::Time(header.timeEndSeconds,
                                                    1000 * header.timeEndMicroSeconds);
    const ros::Time scan_time((end_absolute_time - start_absolute_time).toSec());

    const float angle_start = 1e-6 * static_cast<float>(header.spindleAngleStart);
    const float angle_end   = 1e-6 * static_cast<float>(header.spindleAngleEnd);

    //
    // Initialize the previous scan time to the start time if it has not
    // been previously set

    if (previous_scan_time_.is_zero())
    {
        previous_scan_time_ = start_absolute_time;
    }


    //
    // Compute the velocity between our last scan and the start of our current
    // scan

    float velocity = angles::normalize_angle((angle_start - spindle_angle_)) /
        (start_absolute_time - previous_scan_time_).toSec();

    publishSpindleTransform(angle_start, velocity, start_absolute_time);
    spindle_angle_ = angle_start;

    //
    // Compute the velocity for the spindle during the duration of our
    // laser scan

    velocity = angles::normalize_angle((angle_end - angle_start)) / scan_time.toSec();

    publishSpindleTransform(angle_end, velocity, end_absolute_time);
    spindle_angle_ = angle_end;
    previous_scan_time_ = end_absolute_time;

    if (scan_pub_.getNumSubscribers() > 0) {

        const double arcRadians = 1e-6 * static_cast<double>(header.scanArc);

        laser_msg_.header.frame_id = frame_id_;
        laser_msg_.header.stamp    = start_absolute_time;
        laser_msg_.scan_time       = scan_time.toSec();
        laser_msg_.time_increment  = laser_msg_.scan_time / header.pointCount;
        laser_msg_.angle_min       = -arcRadians / 2.0;
        laser_msg_.angle_max       = arcRadians / 2.0;
        laser_msg_.angle_increment = arcRadians / (header.pointCount - 1);
        laser_msg_.range_min       = 0.0;
        laser_msg_.range_max       = static_cast<double>(header.maxRange) / 1000.0;

        laser_msg_.ranges.resize(header.pointCount);
        laser_msg_.intensities.resize(header.pointCount);

        for (size_t i=0; i<header.pointCount; i++) {
            laser_msg_.ranges[i]      = 1e-3 * static_cast<float>(header.rangesP[i]); // from millimeters
            laser_msg_.intensities[i] = static_cast<float>(header.intensitiesP[i]);   // in device units
        }

        scan_pub_.publish(laser_msg_);
    }

    if (raw_lidar_data_pub_.getNumSubscribers() > 0) {

        RawLidarData::Ptr ros_msg(new RawLidarData);

        ros_msg->scan_count  = header.scanId;
        ros_msg->time_start  = start_absolute_time;
        ros_msg->time_end    = end_absolute_time;
        ros_msg->angle_start = header.spindleAngleStart;
        ros_msg->angle_end   = header.spindleAngleEnd;

        ros_msg->distance.resize(header.pointCount);
        memcpy(&(ros_msg->distance[0]),
               header.rangesP,
               header.pointCount * sizeof(uint32_t));

        ros_msg->intensity.resize(header.pointCount);
        memcpy(&(ros_msg->intensity[0]),
               header.intensitiesP,
               header.pointCount * sizeof(uint32_t));

        raw_lidar_data_pub_.publish(ros_msg);
    }
}

void Laser::publishSpindleTransform(const float spindle_angle, const float velocity, const ros::Time& time) {
    joint_states_.header.stamp = time;
    joint_states_.position[0] = spindle_angle;
    joint_states_.velocity[0] = velocity;
    joint_states_pub_.publish(joint_states_);
}

tf2::Transform Laser::getSpindleTransform(float spindle_angle){

    //
    // Spindle angle turns about the z-axis to create a transform where it adjusts
    // yaw
    tf2::Matrix3x3 spindle_rot = tf2::Matrix3x3();
    spindle_rot.setRPY(0.0, 0.0, spindle_angle);
    tf2::Transform spindle_to_motor = tf2::Transform(spindle_rot);

    return spindle_to_motor;
}

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

    //
    // If our message time is 0 or our message time is over 1 second old
    // we are not subscribed to a laser topic anymore. Publish the default
    // transform
    if ( (laser_msg_.header.stamp.is_zero() ||
         (ros::Time::now() - laser_msg_.header.stamp >= ros::Duration(1))) &&
         (point_cloud_.header.stamp.is_zero() ||
         (ros::Time::now() - point_cloud_.header.stamp >= ros::Duration(1))) )

    {
        publishSpindleTransform(spindle_angle_, 0.0, ros::Time::now());
    }
}

void Laser::stop()
{
    subscribers_ = 0;

    Status status = driver_->stopStreams(Source_Lidar_Scan);
    if (Status_Ok != status)
        ROS_ERROR("Laser: failed to stop laser stream: %s",
                  Channel::statusString(status));
}

void Laser::unsubscribe()
{
    std::lock_guard<std::mutex> lock(sub_lock_);

    if (--subscribers_ > 0)
        return;

    stop();
}

void Laser::subscribe()
{
    std::lock_guard<std::mutex> lock(sub_lock_);

    if (0 == subscribers_++) {

        Status status = driver_->startStreams(Source_Lidar_Scan);
        if (Status_Ok != status)
            ROS_ERROR("Laser: failed to start laser stream: %s",
                      Channel::statusString(status));
    }
}
}