laser.cpp
Go to the documentation of this file.
00001 
00034 #include <multisense_ros/laser.h>
00035 #include <multisense_ros/RawLidarData.h>
00036 #include <multisense_ros/RawLidarCal.h>
00037 #include <angles/angles.h>
00038 
00039 #include <arpa/inet.h>
00040 
00041 using namespace crl::multisense;
00042 
00043 namespace { // anonymous
00044 
00045 const uint32_t laser_cloud_step = 16;
00046 
00047 tf::Transform makeTransform(float T[4][4])
00048 {
00049     //
00050     // Manually create the rotation matrix
00051     tf::Matrix3x3 rot = tf::Matrix3x3(T[0][0],
00052                                       T[0][1],
00053                                       T[0][2],
00054                                       T[1][0],
00055                                       T[1][1],
00056                                       T[1][2],
00057                                       T[2][0],
00058                                       T[2][1],
00059                                       T[2][2]);
00060 
00061     //
00062     // Maually create the translation vector
00063     tf::Vector3 trans = tf::Vector3(T[0][3], T[1][3], T[2][3]);
00064 
00065     return tf::Transform(rot, trans);
00066 }
00067 
00068 
00069 
00070 }; // anonymous
00071 
00072 namespace multisense_ros {
00073 
00074 const float Laser::EXPECTED_RATE = 40.0;
00075 
00076 namespace { // anonymous
00077 
00078 //
00079 // Shims for c-style driver callbacks
00080 
00081 void lCB(const lidar::Header&        header,
00082          void*                       userDataP)
00083 {
00084     reinterpret_cast<Laser*>(userDataP)->scanCallback(header);
00085 }
00086 
00087 void pCB(const lidar::Header&        header,
00088          void*                       userDataP)
00089 {
00090     reinterpret_cast<Laser*>(userDataP)->pointCloudCallback(header);
00091 }
00092 
00093 }; // anonymous
00094 
00095 Laser::Laser(Channel* driver,
00096              const std::string& tf_prefix):
00097     driver_(driver),
00098     subscribers_(0),
00099     spindle_angle_(0.0),
00100     previous_scan_time_(0.0)
00101 
00102 {
00103 
00104     //
00105     // Get device info
00106 
00107     system::DeviceInfo  deviceInfo;
00108 
00109     Status status = driver_->getDeviceInfo(deviceInfo);
00110     if (Status_Ok != status) {
00111         ROS_ERROR("Laser: failed to query device info: %s",
00112                   Channel::statusString(status));
00113         return;
00114     }
00115 
00116     switch(deviceInfo.hardwareRevision) {
00117     case system::DeviceInfo::HARDWARE_REV_MULTISENSE_SL:
00118 
00119         ; // ok, this one has a laser
00120 
00121         break;
00122     default:
00123 
00124         ROS_INFO("hardware does not support a laser");
00125         return;
00126     }
00127 
00128     ros::NodeHandle nh("");
00129 
00130     //
00131     // Set frame ID
00132 
00133     frame_id_ = tf_prefix + "/head_hokuyo_frame";
00134 
00135     left_camera_optical_ =  tf_prefix + "/" + "left_camera_optical_frame";
00136     motor_               =  tf_prefix + "/" + "motor";
00137     spindle_             =  tf_prefix + "/" + "spindle";
00138     hokuyo_              =  tf_prefix + "/" + "hokuyo_link";
00139 
00140     ROS_INFO("laser frame id: %s", frame_id_.c_str());
00141 
00142     //
00143     // Stop lidar stream
00144 
00145     stop();
00146 
00147     //
00148     // Query calibration from sensor
00149 
00150     status = driver_->getLidarCalibration(lidar_cal_);
00151     if (Status_Ok != status)
00152         ROS_WARN("could not query lidar calibration (%s), using URDF defaults",
00153                  Channel::statusString(status));
00154     else {
00155 
00156         //
00157         // Create two static transforms representing sensor calibration
00158         motor_to_camera_ = makeTransform(lidar_cal_.cameraToSpindleFixed);
00159         laser_to_spindle_ = makeTransform(lidar_cal_.laserToSpindle);
00160 
00161     }
00162 
00163     //
00164     // Create scan publisher
00165 
00166     scan_pub_ = nh.advertise<sensor_msgs::LaserScan>("lidar_scan", 20,
00167                 boost::bind(&Laser::subscribe, this),
00168                 boost::bind(&Laser::unsubscribe, this));
00169 
00170     //
00171     // Initialize point cloud structure
00172 
00173     point_cloud_.is_bigendian    = (htonl(1) == 1);
00174     point_cloud_.is_dense        = true;
00175     point_cloud_.point_step      = laser_cloud_step;
00176     point_cloud_.height          = 1;
00177     point_cloud_.header.frame_id =  tf_prefix + "/left_camera_optical_frame";
00178 
00179     point_cloud_.fields.resize(4);
00180     point_cloud_.fields[0].name     = "x";
00181     point_cloud_.fields[0].offset   = 0;
00182     point_cloud_.fields[0].count    = 1;
00183     point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00184     point_cloud_.fields[1].name     = "y";
00185     point_cloud_.fields[1].offset   = 4;
00186     point_cloud_.fields[1].count    = 1;
00187     point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00188     point_cloud_.fields[2].name     = "z";
00189     point_cloud_.fields[2].offset   = 8;
00190     point_cloud_.fields[2].count    = 1;
00191     point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00192     point_cloud_.fields[3].name     = "intensity";
00193     point_cloud_.fields[3].offset   = 12;
00194     point_cloud_.fields[3].count    = 1;
00195     point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
00196 
00197     //
00198     // Create point cloud publisher
00199 
00200     point_cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("lidar_points2", 5,
00201                        boost::bind(&Laser::subscribe, this),
00202                        boost::bind(&Laser::unsubscribe, this));
00203 
00204     //
00205     // Create calibration publishers
00206 
00207     ros::NodeHandle calibration_nh(nh, "calibration");
00208     raw_lidar_cal_pub_  = calibration_nh.advertise<multisense_ros::RawLidarCal>("raw_lidar_cal", 1, true);
00209     raw_lidar_data_pub_ = calibration_nh.advertise<multisense_ros::RawLidarData>("raw_lidar_data", 20,
00210                           boost::bind(&Laser::subscribe, this),
00211                           boost::bind(&Laser::unsubscribe, this));
00212 
00213     //
00214     // Publish calibration
00215 
00216     multisense_ros::RawLidarCal ros_msg;
00217 
00218     const float *calP = reinterpret_cast<const float*>(&(lidar_cal_.laserToSpindle[0][0]));
00219     for(uint32_t i=0; i<16; ++i)
00220         ros_msg.laserToSpindle[i] = calP[i];
00221 
00222     calP = reinterpret_cast<const float*>(&(lidar_cal_.cameraToSpindleFixed[0][0]));
00223     for(uint32_t i=0; i<16; ++i)
00224         ros_msg.cameraToSpindleFixed[i] = calP[i];
00225 
00226     raw_lidar_cal_pub_.publish(ros_msg);
00227 
00228 
00229     //
00230     // Populate the jointstates message for publishing the laser spindle
00231     // angle
00232 
00233     joint_states_.name.resize(1);
00234     joint_states_.position.resize(1);
00235     joint_states_.velocity.resize(1);
00236     joint_states_.effort.resize(1);
00237     joint_states_.name[0] = "motor_joint";
00238     joint_states_.position[0] = 0.0;
00239     joint_states_.velocity[0] = 0.0;
00240     joint_states_.effort[0] = 0.0;
00241 
00242     //
00243     // Create a joint state publisher
00244 
00245     joint_states_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states", 1, true);
00246 
00247     //
00248     // Create a timer routine to publish the laser transform even when nothing
00249     // is subscribed to the laser topics. Publishing occurs at 1Hz
00250 
00251     timer_ = nh.createTimer(ros::Duration(1), &Laser::defaultTfPublisher, this);
00252 
00253     //
00254     // Register callbacks, driver creates dedicated background thread for each
00255 
00256     driver_->addIsolatedCallback(lCB, this);
00257     driver_->addIsolatedCallback(pCB, this);
00258 
00259 
00260 }
00261 
00262 Laser::~Laser()
00263 {
00264     boost::mutex::scoped_lock lock(sub_lock_);
00265     stop();
00266     driver_->removeIsolatedCallback(lCB);
00267     driver_->removeIsolatedCallback(pCB);
00268 }
00269 
00270 
00271 void Laser::pointCloudCallback(const lidar::Header& header)
00272 {
00273     //
00274     // Get out if we have no work to do
00275 
00276     if (0 == point_cloud_pub_.getNumSubscribers())
00277         return;
00278 
00279     point_cloud_.data.resize(laser_cloud_step * header.pointCount);
00280     point_cloud_.row_step     = header.pointCount * laser_cloud_step;
00281     point_cloud_.width        = header.pointCount;
00282     point_cloud_.header.stamp = ros::Time(header.timeStartSeconds,
00283                                           1000 * header.timeStartMicroSeconds);
00284     //
00285     // For convenience below
00286 
00287     uint8_t       *cloudP            = reinterpret_cast<uint8_t*>(&point_cloud_.data[0]);
00288     const uint32_t pointSize         = 3 * sizeof(float); // x, y, z
00289     const double   arcRadians        = 1e-6 * static_cast<double>(header.scanArc);
00290     const double   mirrorThetaStart  = -arcRadians / 2.0;
00291     const double   spindleAngleStart = angles::normalize_angle(1e-6 * static_cast<double>(header.spindleAngleStart));
00292     const double   spindleAngleEnd   = angles::normalize_angle(1e-6 * static_cast<double>(header.spindleAngleEnd));
00293     const double   spindleAngleRange = angles::normalize_angle(spindleAngleEnd - spindleAngleStart);
00294 
00295     for(uint32_t i=0; i<header.pointCount; ++i, cloudP += laser_cloud_step) {
00296 
00297         //
00298         // Percent through the scan arc
00299 
00300         const double percent = static_cast<double>(i) / static_cast<double>(header.pointCount - 1);
00301 
00302         //
00303         // The mirror angle for this point
00304 
00305         const double mirrorTheta = mirrorThetaStart + percent * arcRadians;
00306 
00307         //
00308         // The rotation about the spindle
00309 
00310         const double spindleTheta = spindleAngleStart + percent * spindleAngleRange;
00311 
00312         tf::Transform spindle_to_motor = getSpindleTransform(spindleTheta);
00313 
00314         //
00315         // The coordinate in left optical frame
00316 
00317         const double      rangeMeters = 1e-3 * static_cast<double>(header.rangesP[i]);  // from millimeters
00318         const tf::Vector3 pointMotor  = (laser_to_spindle_ *
00319                                          tf::Vector3(rangeMeters * std::sin(mirrorTheta), 0.0,
00320                                                      rangeMeters *  std::cos(mirrorTheta)));
00321         const tf::Vector3 pointCamera = motor_to_camera_ * (spindle_to_motor * pointMotor);
00322 
00323         //
00324         // Copy data to point cloud structure
00325 
00326         const float xyz[3] = {static_cast<float>(pointCamera.getX()),
00327                               static_cast<float>(pointCamera.getY()),
00328                               static_cast<float>(pointCamera.getZ())};
00329 
00330         memcpy(cloudP, &(xyz[0]), pointSize);
00331         float* intensityChannel = reinterpret_cast<float*>(cloudP + pointSize);
00332         *intensityChannel = static_cast<float>(header.intensitiesP[i]);   // in device units
00333     }
00334 
00335     point_cloud_pub_.publish(point_cloud_);
00336 }
00337 
00338 void Laser::scanCallback(const lidar::Header& header)
00339 {
00340 
00341     const ros::Time start_absolute_time = ros::Time(header.timeStartSeconds,
00342                                                     1000 * header.timeStartMicroSeconds);
00343     const ros::Time end_absolute_time   = ros::Time(header.timeEndSeconds,
00344                                                     1000 * header.timeEndMicroSeconds);
00345     const ros::Time scan_time((end_absolute_time - start_absolute_time).toSec());
00346 
00347     const float angle_start = 1e-6 * static_cast<float>(header.spindleAngleStart);
00348     const float angle_end   = 1e-6 * static_cast<float>(header.spindleAngleEnd);
00349 
00350     publishStaticTransforms(start_absolute_time);
00351 
00352     //
00353     // Initialize the previous scan time to the start time if it has not
00354     // been previously set
00355 
00356     if (previous_scan_time_.is_zero())
00357     {
00358         previous_scan_time_ = start_absolute_time;
00359     }
00360 
00361 
00362     //
00363     // Compute the velocity between our last scan and the start of our current
00364     // scan
00365 
00366     float velocity = angles::normalize_angle((angle_start - spindle_angle_)) /
00367         (start_absolute_time - previous_scan_time_).toSec();
00368 
00369     publishSpindleTransform(angle_start, velocity, start_absolute_time);
00370     spindle_angle_ = angle_start;
00371 
00372     //
00373     // Compute the velocity for the spindle during the duration of our
00374     // laser scan
00375 
00376     velocity = angles::normalize_angle((angle_end - angle_start)) / scan_time.toSec();
00377 
00378     publishSpindleTransform(angle_end, velocity, end_absolute_time);
00379     spindle_angle_ = angle_end;
00380     previous_scan_time_ = end_absolute_time;
00381 
00382     if (scan_pub_.getNumSubscribers() > 0) {
00383 
00384         const double arcRadians = 1e-6 * static_cast<double>(header.scanArc);
00385 
00386         laser_msg_.header.frame_id = frame_id_;
00387         laser_msg_.header.stamp    = start_absolute_time;
00388         laser_msg_.scan_time       = scan_time.toSec();
00389         laser_msg_.time_increment  = laser_msg_.scan_time / header.pointCount;
00390         laser_msg_.angle_min       = -arcRadians / 2.0;
00391         laser_msg_.angle_max       = arcRadians / 2.0;
00392         laser_msg_.angle_increment = arcRadians / (header.pointCount - 1);
00393         laser_msg_.range_min       = 0.0;
00394         laser_msg_.range_max       = static_cast<double>(header.maxRange) / 1000.0;
00395 
00396         laser_msg_.ranges.resize(header.pointCount);
00397         laser_msg_.intensities.resize(header.pointCount);
00398 
00399         for (size_t i=0; i<header.pointCount; i++) {
00400             laser_msg_.ranges[i]      = 1e-3 * static_cast<float>(header.rangesP[i]); // from millimeters
00401             laser_msg_.intensities[i] = static_cast<float>(header.intensitiesP[i]);   // in device units
00402         }
00403 
00404         scan_pub_.publish(laser_msg_);
00405     }
00406 
00407     if (raw_lidar_data_pub_.getNumSubscribers() > 0) {
00408 
00409         RawLidarData::Ptr ros_msg(new RawLidarData);
00410 
00411         ros_msg->scan_count  = header.scanId;
00412         ros_msg->time_start  = start_absolute_time;
00413         ros_msg->time_end    = end_absolute_time;
00414         ros_msg->angle_start = header.spindleAngleStart;
00415         ros_msg->angle_end   = header.spindleAngleEnd;
00416 
00417         ros_msg->distance.resize(header.pointCount);
00418         memcpy(&(ros_msg->distance[0]),
00419                header.rangesP,
00420                header.pointCount * sizeof(uint32_t));
00421 
00422         ros_msg->intensity.resize(header.pointCount);
00423         memcpy(&(ros_msg->intensity[0]),
00424                header.intensitiesP,
00425                header.pointCount * sizeof(uint32_t));
00426 
00427         raw_lidar_data_pub_.publish(ros_msg);
00428     }
00429 }
00430 
00431 void Laser::publishStaticTransforms(const ros::Time& time) {
00432 
00433     //
00434     // Publish the static transforms from our calibration.
00435     static_tf_broadcaster_.sendTransform(tf::StampedTransform(motor_to_camera_,
00436                                           time,left_camera_optical_,
00437                                           motor_));
00438 
00439 
00440 
00441     static_tf_broadcaster_.sendTransform(tf::StampedTransform(laser_to_spindle_,
00442                                           time, spindle_, hokuyo_));
00443 
00444 
00445 }
00446 
00447 void Laser::publishSpindleTransform(const float spindle_angle, const float velocity, const ros::Time& time) {
00448     joint_states_.header.stamp = time;
00449     joint_states_.position[0] = spindle_angle;
00450     joint_states_.velocity[0] = velocity;
00451     joint_states_pub_.publish(joint_states_);
00452 }
00453 
00454 tf::Transform Laser::getSpindleTransform(float spindle_angle){
00455 
00456     //
00457     // Spindle angle turns about the z-axis to create a transform where it adjusts
00458     // yaw
00459     tf::Matrix3x3 spindle_rot = tf::Matrix3x3();
00460     spindle_rot.setRPY(0.0, 0.0, spindle_angle);
00461     tf::Transform spindle_to_motor = tf::Transform(spindle_rot);
00462 
00463     return spindle_to_motor;
00464 }
00465 
00466 void Laser::defaultTfPublisher(const ros::TimerEvent& event){
00467     //
00468     // If our message time is 0 or our message time is over 1 second old
00469     // we are not subscribed to a laser topic anymore. Publish the default
00470     // transform
00471     if ( (laser_msg_.header.stamp.is_zero() ||
00472          (ros::Time::now() - laser_msg_.header.stamp >= ros::Duration(1))) &&
00473          (point_cloud_.header.stamp.is_zero() ||
00474          (ros::Time::now() - point_cloud_.header.stamp >= ros::Duration(1))) )
00475 
00476     {
00477         publishStaticTransforms(ros::Time::now());
00478         publishSpindleTransform(spindle_angle_, 0.0, ros::Time::now());
00479     }
00480 }
00481 
00482 void Laser::stop()
00483 {
00484     subscribers_ = 0;
00485 
00486     Status status = driver_->stopStreams(Source_Lidar_Scan);
00487     if (Status_Ok != status)
00488         ROS_ERROR("Laser: failed to stop laser stream: %s",
00489                   Channel::statusString(status));
00490 }
00491 
00492 void Laser::unsubscribe()
00493 {
00494     boost::mutex::scoped_lock lock(sub_lock_);
00495 
00496     if (--subscribers_ > 0)
00497         return;
00498 
00499     stop();
00500 }
00501 
00502 void Laser::subscribe()
00503 {
00504     boost::mutex::scoped_lock lock(sub_lock_);
00505 
00506     if (0 == subscribers_++) {
00507 
00508         Status status = driver_->startStreams(Source_Lidar_Scan);
00509         if (Status_Ok != status)
00510             ROS_ERROR("Laser: failed to start laser stream: %s",
00511                       Channel::statusString(status));
00512     }
00513 }
00514 }


multisense_ros
Author(s):
autogenerated on Thu Aug 27 2015 14:01:22