hdl_grabber.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012 The MITRE Corporation
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 
00038 #include <pcl/console/print.h>
00039 #include <pcl/io/boost.h>
00040 #include <pcl/io/hdl_grabber.h>
00041 #include <boost/version.hpp>
00042 #include <boost/foreach.hpp>
00043 #include <boost/property_tree/ptree.hpp>
00044 #include <boost/property_tree/xml_parser.hpp>
00045 #include <boost/array.hpp>
00046 #include <boost/bind.hpp>
00047 #include <boost/math/special_functions.hpp>
00048 #ifdef HAVE_PCAP
00049 #include <pcap.h>
00050 #endif // #ifdef HAVE_PCAP
00051 
00052 const boost::asio::ip::address pcl::HDLGrabber::HDL_DEFAULT_NETWORK_ADDRESS = boost::asio::ip::address::from_string ("192.168.3.255");
00053 double *pcl::HDLGrabber::cos_lookup_table_ = NULL;
00054 double *pcl::HDLGrabber::sin_lookup_table_ = NULL;
00055 
00056 using boost::asio::ip::udp;
00057 
00059 pcl::HDLGrabber::HDLGrabber (const std::string& correctionsFile,
00060                              const std::string& pcapFile) 
00061   : hdl_data_ ()
00062   , udp_listener_endpoint_ (HDL_DEFAULT_NETWORK_ADDRESS, HDL_DATA_PORT)
00063   , source_address_filter_ ()
00064   , source_port_filter_ (443)
00065   , hdl_read_socket_service_ ()
00066   , hdl_read_socket_ (NULL)
00067   , pcap_file_name_ (pcapFile)
00068   , queue_consumer_thread_ (NULL)
00069   , hdl_read_packet_thread_ (NULL)
00070   , current_scan_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ())
00071   , current_sweep_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ())
00072   , current_scan_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ())
00073   , current_sweep_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ())
00074   , current_scan_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ())
00075   , current_sweep_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ())
00076   , last_azimuth_ (65000)
00077   , sweep_xyz_signal_ ()
00078   , sweep_xyzrgb_signal_ ()
00079   , sweep_xyzi_signal_ ()
00080   , scan_xyz_signal_ ()
00081   , scan_xyzrgb_signal_ ()
00082   , scan_xyzi_signal_ ()
00083   , min_distance_threshold_(0.0)
00084   , max_distance_threshold_(10000.0)
00085 {
00086   initialize (correctionsFile);
00087 }
00088 
00090 pcl::HDLGrabber::HDLGrabber (const boost::asio::ip::address& ipAddress,
00091                              const unsigned short int port, 
00092                              const std::string& correctionsFile) 
00093   : hdl_data_ ()
00094   , udp_listener_endpoint_ (ipAddress, port)
00095   , source_address_filter_ ()
00096   , source_port_filter_ (443)
00097   , hdl_read_socket_service_ ()
00098   , hdl_read_socket_ (NULL)
00099   , pcap_file_name_ ()
00100   , queue_consumer_thread_ (NULL)
00101   , hdl_read_packet_thread_ (NULL)
00102   , current_scan_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ())
00103   , current_sweep_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ())
00104   , current_scan_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ())
00105   , current_sweep_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ())
00106   , current_scan_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ())
00107   , current_sweep_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ())
00108   , last_azimuth_ (65000)
00109   , sweep_xyz_signal_ ()
00110   , sweep_xyzrgb_signal_ ()
00111   , sweep_xyzi_signal_ ()
00112   , scan_xyz_signal_ ()
00113   , scan_xyzrgb_signal_ ()
00114   , scan_xyzi_signal_ ()
00115   , min_distance_threshold_(0.0)
00116   , max_distance_threshold_(10000.0)
00117 {
00118   initialize (correctionsFile);
00119 }
00120 
00122 pcl::HDLGrabber::~HDLGrabber () throw ()
00123 {
00124   stop ();
00125 
00126   disconnect_all_slots<sig_cb_velodyne_hdl_sweep_point_cloud_xyz> ();
00127   disconnect_all_slots<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb> ();
00128   disconnect_all_slots<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi> ();
00129   disconnect_all_slots<sig_cb_velodyne_hdl_scan_point_cloud_xyz> ();
00130   disconnect_all_slots<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb> ();
00131   disconnect_all_slots<sig_cb_velodyne_hdl_scan_point_cloud_xyzi> ();
00132 }
00133 
00135 void
00136 pcl::HDLGrabber::initialize (const std::string& correctionsFile)
00137 {
00138   if (cos_lookup_table_ == NULL && sin_lookup_table_ == NULL)
00139   {
00140     cos_lookup_table_ = static_cast<double *> (malloc (HDL_NUM_ROT_ANGLES * sizeof (*cos_lookup_table_)));
00141     sin_lookup_table_ = static_cast<double *> (malloc (HDL_NUM_ROT_ANGLES * sizeof (*sin_lookup_table_)));
00142     for (unsigned int i = 0; i < HDL_NUM_ROT_ANGLES; i++)
00143     {
00144       double rad = (M_PI / 180.0) * (static_cast<double> (i) / 100.0);
00145       cos_lookup_table_[i] = std::cos (rad);
00146       sin_lookup_table_[i] = std::sin (rad);
00147     }
00148   }
00149 
00150   loadCorrectionsFile (correctionsFile);
00151 
00152   for (int i = 0; i < HDL_MAX_NUM_LASERS; i++)
00153   {
00154     HDLLaserCorrection correction = laser_corrections_[i];
00155     laser_corrections_[i].sinVertOffsetCorrection = correction.verticalOffsetCorrection
00156                                        * correction.sinVertCorrection;
00157     laser_corrections_[i].cosVertOffsetCorrection = correction.verticalOffsetCorrection
00158                                        * correction.cosVertCorrection;
00159   }
00160   sweep_xyz_signal_ = createSignal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz> ();
00161   sweep_xyzrgb_signal_ = createSignal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb> ();
00162   sweep_xyzi_signal_ =createSignal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi> ();
00163   scan_xyz_signal_ = createSignal<sig_cb_velodyne_hdl_scan_point_cloud_xyz> ();
00164   scan_xyzrgb_signal_ = createSignal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb> ();
00165   scan_xyzi_signal_ = createSignal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi> ();
00166 
00167   current_scan_xyz_.reset (new pcl::PointCloud<pcl::PointXYZ>);
00168   current_scan_xyzi_.reset (new pcl::PointCloud<pcl::PointXYZI>);
00169   current_sweep_xyz_.reset (new pcl::PointCloud<pcl::PointXYZ>);
00170   current_sweep_xyzi_.reset (new pcl::PointCloud<pcl::PointXYZI>);
00171 
00172   for (int i = 0; i < HDL_MAX_NUM_LASERS; i++)
00173     laser_rgb_mapping_[i].r = laser_rgb_mapping_[i].g = laser_rgb_mapping_[i].b = 0;
00174 
00175   if (laser_corrections_[32].distanceCorrection == 0.0)
00176   {
00177     for (int i = 0; i < 16; i++)
00178     {
00179       laser_rgb_mapping_[i * 2].b = static_cast<uint8_t> (i * 6 + 64);
00180       laser_rgb_mapping_[i * 2 + 1].b = static_cast<uint8_t> ( (i + 16) * 6 + 64);
00181     }
00182   }
00183   else
00184   {
00185     for (int i = 0; i < 16; i++)
00186     {
00187       laser_rgb_mapping_[i * 2].b = static_cast<uint8_t> (i * 3 + 64);
00188       laser_rgb_mapping_[i * 2 + 1].b = static_cast<uint8_t> ( (i + 16) * 3 + 64);
00189     }
00190     for (int i = 0; i < 16; i++)
00191     {
00192       laser_rgb_mapping_[i * 2 + 32].b = static_cast<uint8_t> (i * 3 + 160);
00193       laser_rgb_mapping_[i * 2 + 33].b = static_cast<uint8_t> ( (i + 16) * 3 + 160);
00194     }
00195   }
00196 }
00197 
00199 void
00200 pcl::HDLGrabber::loadCorrectionsFile (const std::string& correctionsFile)
00201 {
00202   if (correctionsFile.empty ())
00203   {
00204     loadHDL32Corrections ();
00205     return;
00206   }
00207 
00208   boost::property_tree::ptree pt;
00209   try
00210   {
00211     read_xml (correctionsFile, pt, boost::property_tree::xml_parser::trim_whitespace);
00212   }
00213   catch (boost::exception const&)
00214   {
00215     PCL_ERROR ("[pcl::HDLGrabber::loadCorrectionsFile] Error reading calibration file %s!\n", correctionsFile.c_str ());
00216     return;
00217   }
00218 
00219   BOOST_FOREACH (boost::property_tree::ptree::value_type &v, pt.get_child ("boost_serialization.DB.points_"))
00220   {
00221     if (v.first == "item")
00222     {
00223       boost::property_tree::ptree points = v.second;
00224       BOOST_FOREACH(boost::property_tree::ptree::value_type &px, points)
00225       {
00226         if (px.first == "px")
00227         {
00228           boost::property_tree::ptree calibrationData = px.second;
00229           int index = -1;
00230           double azimuth = 0, vertCorrection = 0, distCorrection = 0,
00231                  vertOffsetCorrection = 0, horizOffsetCorrection = 0;
00232 
00233           BOOST_FOREACH (boost::property_tree::ptree::value_type &item, calibrationData)
00234           {
00235             if (item.first == "id_")
00236               index = atoi (item.second.data ().c_str ());
00237             if (item.first == "rotCorrection_")
00238               azimuth = atof (item.second.data ().c_str ());
00239             if (item.first == "vertCorrection_")
00240               vertCorrection = atof (item.second.data ().c_str ());
00241             if (item.first == "distCorrection_")
00242               distCorrection = atof (item.second.data ().c_str ());
00243             if (item.first == "vertOffsetCorrection_")
00244               vertOffsetCorrection = atof (item.second.data ().c_str ());
00245             if (item.first == "horizOffsetCorrection_")
00246               horizOffsetCorrection = atof (item.second.data ().c_str ());
00247           }
00248           if (index != -1)
00249           {
00250             laser_corrections_[index].azimuthCorrection = azimuth;
00251             laser_corrections_[index].verticalCorrection = vertCorrection;
00252             laser_corrections_[index].distanceCorrection = distCorrection / 100.0;
00253             laser_corrections_[index].verticalOffsetCorrection = vertOffsetCorrection / 100.0;
00254             laser_corrections_[index].horizontalOffsetCorrection = horizOffsetCorrection / 100.0;
00255 
00256             laser_corrections_[index].cosVertCorrection = std::cos (HDL_Grabber_toRadians(laser_corrections_[index].verticalCorrection));
00257             laser_corrections_[index].sinVertCorrection = std::sin (HDL_Grabber_toRadians(laser_corrections_[index].verticalCorrection));
00258           }
00259         }
00260       }
00261     }
00262   }
00263 }
00264 
00266 void
00267 pcl::HDLGrabber::loadHDL32Corrections ()
00268 {
00269   double hdl32VerticalCorrections[] = { 
00270     -30.67, -9.3299999, -29.33, -8, -28,
00271     -6.6700001, -26.67, -5.3299999, -25.33, -4, -24, -2.6700001, -22.67,
00272     -1.33, -21.33, 0, -20, 1.33, -18.67, 2.6700001, -17.33, 4, -16, 5.3299999,
00273     -14.67, 6.6700001, -13.33, 8, -12, 9.3299999, -10.67, 10.67 };
00274   for (int i = 0; i < HDL_LASER_PER_FIRING; i++)
00275   {
00276     laser_corrections_[i].azimuthCorrection = 0.0;
00277     laser_corrections_[i].distanceCorrection = 0.0;
00278     laser_corrections_[i].horizontalOffsetCorrection = 0.0;
00279     laser_corrections_[i].verticalOffsetCorrection = 0.0;
00280     laser_corrections_[i].verticalCorrection = hdl32VerticalCorrections[i];
00281     laser_corrections_[i].sinVertCorrection = std::sin (HDL_Grabber_toRadians(hdl32VerticalCorrections[i]));
00282     laser_corrections_[i].cosVertCorrection = std::cos (HDL_Grabber_toRadians(hdl32VerticalCorrections[i]));
00283   }
00284   for (int i = HDL_LASER_PER_FIRING; i < HDL_MAX_NUM_LASERS; i++)
00285   {
00286     laser_corrections_[i].azimuthCorrection = 0.0;
00287     laser_corrections_[i].distanceCorrection = 0.0;
00288     laser_corrections_[i].horizontalOffsetCorrection = 0.0;
00289     laser_corrections_[i].verticalOffsetCorrection = 0.0;
00290     laser_corrections_[i].verticalCorrection = 0.0;
00291     laser_corrections_[i].sinVertCorrection = 0.0;
00292     laser_corrections_[i].cosVertCorrection = 1.0;
00293   }
00294 }
00295 
00297 void
00298 pcl::HDLGrabber::processVelodynePackets ()
00299 {
00300   while (true)
00301   {
00302     unsigned char *data;
00303     if (!hdl_data_.dequeue (data))
00304       return;
00305 
00306     toPointClouds (reinterpret_cast<HDLDataPacket *> (data));
00307 
00308     free (data);
00309   }
00310 }
00311 
00313 void
00314 pcl::HDLGrabber::toPointClouds (HDLDataPacket *dataPacket)
00315 {
00316   static uint32_t scanCounter = 0;
00317   static uint32_t sweepCounter = 0;
00318   if (sizeof (HDLLaserReturn) != 3)
00319     return;
00320 
00321   current_scan_xyz_.reset (new pcl::PointCloud<pcl::PointXYZ> ());
00322   current_scan_xyzrgb_.reset (new pcl::PointCloud<pcl::PointXYZRGBA> ());
00323   current_scan_xyzi_.reset (new pcl::PointCloud<pcl::PointXYZI> ());
00324 
00325   time_t  time_;
00326   time(&time_);
00327   time_t velodyneTime = (time_ & 0x00000000ffffffffl) << 32 | dataPacket->gpsTimestamp;
00328 
00329   current_scan_xyz_->header.stamp = velodyneTime;
00330   current_scan_xyzrgb_->header.stamp = velodyneTime;
00331   current_scan_xyzi_->header.stamp = velodyneTime;
00332   current_scan_xyz_->header.seq = scanCounter;
00333   current_scan_xyzrgb_->header.seq = scanCounter;
00334   current_scan_xyzi_->header.seq = scanCounter;
00335   scanCounter++;
00336 
00337   for (int i = 0; i < HDL_FIRING_PER_PKT; ++i)
00338   {
00339     HDLFiringData firingData = dataPacket->firingData[i];
00340     int offset = (firingData.blockIdentifier == BLOCK_0_TO_31) ? 0 : 32;
00341 
00342     for (int j = 0; j < HDL_LASER_PER_FIRING; j++)
00343     {
00344       if (firingData.rotationalPosition < last_azimuth_)
00345       {
00346         if (current_sweep_xyzrgb_->size () > 0)
00347         {
00348           current_sweep_xyz_->is_dense = current_sweep_xyzrgb_->is_dense = current_sweep_xyzi_->is_dense = false;
00349           current_sweep_xyz_->header.stamp = velodyneTime;
00350           current_sweep_xyzrgb_->header.stamp = velodyneTime;
00351           current_sweep_xyzi_->header.stamp = velodyneTime;
00352           current_sweep_xyz_->header.seq = sweepCounter;
00353           current_sweep_xyzrgb_->header.seq = sweepCounter;
00354           current_sweep_xyzi_->header.seq = sweepCounter;
00355 
00356           sweepCounter++;
00357 
00358           fireCurrentSweep ();
00359         }
00360         current_sweep_xyz_.reset (new pcl::PointCloud<pcl::PointXYZ> ());
00361         current_sweep_xyzrgb_.reset (new pcl::PointCloud<pcl::PointXYZRGBA> ());
00362         current_sweep_xyzi_.reset (new pcl::PointCloud<pcl::PointXYZI> ());
00363       }
00364 
00365       PointXYZ xyz;
00366       PointXYZI xyzi;
00367       PointXYZRGBA xyzrgb;
00368 
00369       computeXYZI (xyzi, firingData.rotationalPosition, firingData.laserReturns[j], laser_corrections_[j + offset]);
00370 
00371       xyz.x = xyzrgb.x = xyzi.x;
00372       xyz.y = xyzrgb.y = xyzi.y;
00373       xyz.z = xyzrgb.z = xyzi.z;
00374 
00375       xyzrgb.rgba = laser_rgb_mapping_[j + offset].rgba;
00376       if ((boost::math::isnan)(xyz.x) ||
00377           (boost::math::isnan)(xyz.y) ||
00378           (boost::math::isnan)(xyz.z)) {
00379         continue;
00380       }
00381 
00382       current_scan_xyz_->push_back (xyz);
00383       current_scan_xyzi_->push_back (xyzi);
00384       current_scan_xyzrgb_->push_back (xyzrgb);
00385 
00386       current_sweep_xyz_->push_back (xyz);
00387       current_sweep_xyzi_->push_back (xyzi);
00388       current_sweep_xyzrgb_->push_back (xyzrgb);
00389 
00390       last_azimuth_ = firingData.rotationalPosition;
00391     }
00392   }
00393 
00394   current_scan_xyz_->is_dense = current_scan_xyzrgb_->is_dense = current_scan_xyzi_->is_dense = true;
00395   fireCurrentScan (dataPacket->firingData[0].rotationalPosition, 
00396                    dataPacket->firingData[11].rotationalPosition);
00397 }
00398 
00400 void
00401 pcl::HDLGrabber::computeXYZI (pcl::PointXYZI& point, int azimuth, 
00402                               HDLLaserReturn laserReturn, HDLLaserCorrection correction)
00403 {
00404   double cosAzimuth, sinAzimuth;
00405 
00406   double distanceM = laserReturn.distance * 0.002;
00407 
00408   if (distanceM < min_distance_threshold_ || distanceM > max_distance_threshold_) {
00409     point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN();
00410     point.intensity = static_cast<float> (laserReturn.intensity);
00411     return;
00412   }
00413 
00414   if (correction.azimuthCorrection == 0)
00415   {
00416     cosAzimuth = cos_lookup_table_[azimuth];
00417     sinAzimuth = sin_lookup_table_[azimuth];
00418   }
00419   else
00420   {
00421     double azimuthInRadians = HDL_Grabber_toRadians ((static_cast<double> (azimuth) / 100.0) - correction.azimuthCorrection);
00422     cosAzimuth = std::cos (azimuthInRadians);
00423     sinAzimuth = std::sin (azimuthInRadians);
00424   }
00425 
00426   distanceM += correction.distanceCorrection;
00427 
00428   double xyDistance = distanceM * correction.cosVertCorrection - correction.sinVertOffsetCorrection;
00429 
00430   point.x = static_cast<float> (xyDistance * sinAzimuth - correction.horizontalOffsetCorrection * cosAzimuth);
00431   point.y = static_cast<float> (xyDistance * cosAzimuth + correction.horizontalOffsetCorrection * sinAzimuth);
00432   point.z = static_cast<float> (distanceM * correction.sinVertCorrection + correction.cosVertOffsetCorrection);
00433   point.intensity = static_cast<float> (laserReturn.intensity);
00434 }
00435 
00437 void
00438 pcl::HDLGrabber::fireCurrentSweep ()
00439 {
00440   if (sweep_xyz_signal_->num_slots () > 0)
00441     sweep_xyz_signal_->operator() (current_sweep_xyz_);
00442   
00443   if (sweep_xyzrgb_signal_->num_slots () > 0)
00444     sweep_xyzrgb_signal_->operator() (current_sweep_xyzrgb_);
00445 
00446   if (sweep_xyzi_signal_->num_slots () > 0)
00447     sweep_xyzi_signal_->operator() (current_sweep_xyzi_);
00448 }
00449 
00451 void
00452 pcl::HDLGrabber::fireCurrentScan (const unsigned short startAngle,
00453     const unsigned short endAngle)
00454 {
00455   const float start = static_cast<float> (startAngle) / 100.0f;
00456   const float end = static_cast<float> (endAngle) / 100.0f;
00457 
00458   if (scan_xyz_signal_->num_slots () > 0)
00459     scan_xyz_signal_->operator () (current_scan_xyz_, start, end);
00460 
00461   if (scan_xyzrgb_signal_->num_slots () > 0)
00462     scan_xyzrgb_signal_->operator () (current_scan_xyzrgb_, start, end);
00463 
00464   if (scan_xyzi_signal_->num_slots () > 0)
00465     scan_xyzi_signal_->operator() (current_scan_xyzi_, start, end);
00466 }
00467 
00469 void
00470 pcl::HDLGrabber::enqueueHDLPacket (const unsigned char *data,
00471     std::size_t bytesReceived)
00472 {
00473   if (bytesReceived == 1206)
00474   {
00475     unsigned char *dup = static_cast<unsigned char *> (malloc (bytesReceived * sizeof(unsigned char)));
00476     memcpy (dup, data, bytesReceived * sizeof(unsigned char));
00477 
00478     hdl_data_.enqueue (dup);
00479   }
00480 }
00481 
00483 void
00484 pcl::HDLGrabber::start ()
00485 {
00486   terminate_read_packet_thread_ = false;
00487 
00488   if (isRunning ())
00489     return;
00490 
00491   queue_consumer_thread_ = new boost::thread (boost::bind (&HDLGrabber::processVelodynePackets, this));
00492 
00493   if (pcap_file_name_.empty ())
00494   {
00495     try
00496     {
00497       try {
00498                   hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp_listener_endpoint_);
00499           }
00500           catch (std::exception bind) {
00501                   delete hdl_read_socket_;
00502                   hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp::endpoint(boost::asio::ip::address_v4::any(), udp_listener_endpoint_.port()));
00503           }
00504       hdl_read_socket_service_.run ();
00505     }
00506     catch (std::exception &e)
00507     {
00508                 PCL_ERROR ("[pcl::HDLGrabber::start] Unable to bind to socket! %s\n", e.what());
00509         return;
00510     }
00511     hdl_read_packet_thread_ = new boost::thread (boost::bind (&HDLGrabber::readPacketsFromSocket, this));
00512   }
00513   else
00514   {
00515 #ifdef HAVE_PCAP
00516     hdl_read_packet_thread_ = new boost::thread(boost::bind(&HDLGrabber::readPacketsFromPcap, this));
00517 #endif // #ifdef HAVE_PCAP
00518   }
00519 }
00520 
00522 void
00523 pcl::HDLGrabber::stop ()
00524 {
00525   terminate_read_packet_thread_ = true;
00526   hdl_data_.stopQueue ();
00527 
00528   if (hdl_read_packet_thread_ != NULL)
00529   {
00530     hdl_read_packet_thread_->interrupt ();
00531     hdl_read_packet_thread_->join ();
00532     delete hdl_read_packet_thread_;
00533     hdl_read_packet_thread_ = NULL;
00534   }
00535   if (queue_consumer_thread_ != NULL)
00536   {
00537     queue_consumer_thread_->join ();
00538     delete queue_consumer_thread_;
00539     queue_consumer_thread_ = NULL;
00540   }
00541 
00542   if (hdl_read_socket_ != NULL)
00543   {
00544     delete hdl_read_socket_;
00545     hdl_read_socket_ = NULL;
00546   }
00547 }
00548 
00550 bool
00551 pcl::HDLGrabber::isRunning () const
00552 {
00553         return (!hdl_data_.isEmpty() || (hdl_read_packet_thread_ != NULL && 
00554          !hdl_read_packet_thread_->timed_join (boost::posix_time::milliseconds (10))));
00555 }
00556 
00558 std::string
00559 pcl::HDLGrabber::getName () const
00560 {
00561   return (std::string ("Velodyne High Definition Laser (HDL) Grabber"));
00562 }
00563 
00565 float
00566 pcl::HDLGrabber::getFramesPerSecond () const
00567 {
00568   return (0.0f);
00569 }
00570 
00572 void
00573 pcl::HDLGrabber::filterPackets (const boost::asio::ip::address& ipAddress,
00574                                 const unsigned short port)
00575 {
00576   source_address_filter_ = ipAddress;
00577   source_port_filter_ = port;
00578 }
00579 
00581 void
00582 pcl::HDLGrabber::setLaserColorRGB (const pcl::RGB& color,
00583                                    unsigned int laserNumber)
00584 {
00585   if (laserNumber >= HDL_MAX_NUM_LASERS)
00586     return;
00587 
00588   laser_rgb_mapping_[laserNumber] = color;
00589 }
00590 
00592 bool
00593 pcl::HDLGrabber::isAddressUnspecified (const boost::asio::ip::address& ipAddress)
00594 {
00595 #if BOOST_VERSION>=104700
00596   return (ipAddress.is_unspecified ());
00597 #else
00598   if (ipAddress.is_v4 ())
00599     return (ipAddress.to_v4 ().to_ulong() == 0);
00600 
00601   return (false);
00602 #endif
00603 }
00604 
00606 void
00607 pcl::HDLGrabber::setMaximumDistanceThreshold(float &maxThreshold) {
00608   max_distance_threshold_ = maxThreshold;
00609 }
00610 
00612 void
00613 pcl::HDLGrabber::setMinimumDistanceThreshold(float &minThreshold) {
00614   min_distance_threshold_ = minThreshold;
00615 }
00616 
00618 float
00619 pcl::HDLGrabber::getMaximumDistanceThreshold() {
00620   return(max_distance_threshold_);
00621 }
00622 
00624 float
00625 pcl::HDLGrabber::getMinimumDistanceThreshold() {
00626   return(min_distance_threshold_);
00627 }
00628 
00630 void
00631 pcl::HDLGrabber::readPacketsFromSocket ()
00632 {
00633   unsigned char data[1500];
00634   udp::endpoint sender_endpoint;
00635 
00636   while (!terminate_read_packet_thread_ && hdl_read_socket_->is_open())
00637   {
00638     size_t length = hdl_read_socket_->receive_from (boost::asio::buffer (data, 1500), sender_endpoint);
00639 
00640     if (isAddressUnspecified (source_address_filter_) || 
00641         (source_address_filter_ == sender_endpoint.address () && source_port_filter_ == sender_endpoint.port ()))
00642         {
00643       enqueueHDLPacket (data, length);
00644         }
00645   }
00646 }
00647 
00649 #ifdef HAVE_PCAP
00650 void
00651 pcl::HDLGrabber::readPacketsFromPcap ()
00652 {
00653   struct pcap_pkthdr *header;
00654   const unsigned char *data;
00655   char errbuff[PCAP_ERRBUF_SIZE];
00656 
00657   pcap_t *pcap = pcap_open_offline (pcap_file_name_.c_str (), errbuff);
00658 
00659   struct bpf_program filter;
00660   std::ostringstream stringStream;
00661 
00662   stringStream << "udp ";
00663   if (!isAddressUnspecified(source_address_filter_))
00664   {
00665     stringStream << " and src port " << source_port_filter_ << " and src host " << source_address_filter_.to_string();
00666   }
00667 
00668   // PCAP_NETMASK_UNKNOWN should be 0xffffffff, but it's undefined in older PCAP versions
00669   if (pcap_compile (pcap, &filter, stringStream.str ().c_str(), 0, 0xffffffff) == -1)
00670   {
00671     PCL_WARN ("[pcl::HDLGrabber::readPacketsFromPcap] Issue compiling filter: %s.\n", pcap_geterr (pcap));
00672   }
00673   else if (pcap_setfilter(pcap, &filter) == -1)
00674   {
00675     PCL_WARN ("[pcl::HDLGrabber::readPacketsFromPcap] Issue setting filter: %s.\n", pcap_geterr (pcap));
00676   }
00677 
00678   struct timeval lasttime;
00679   unsigned long long uSecDelay;
00680 
00681   lasttime.tv_sec = 0;
00682 
00683   int returnValue = pcap_next_ex(pcap, &header, &data);
00684 
00685   while (returnValue >= 0 && !terminate_read_packet_thread_)
00686   {
00687     if (lasttime.tv_sec == 0)
00688     {
00689       lasttime.tv_sec = header->ts.tv_sec;
00690       lasttime.tv_usec = header->ts.tv_usec;
00691     }
00692     if (lasttime.tv_usec > header->ts.tv_usec)
00693     {
00694       lasttime.tv_usec -= 1000000;
00695       lasttime.tv_sec++;
00696     }
00697     uSecDelay = ((header->ts.tv_sec - lasttime.tv_sec) * 1000000) +
00698                 (header->ts.tv_usec - lasttime.tv_usec);
00699 
00700     boost::this_thread::sleep(boost::posix_time::microseconds(uSecDelay));
00701 
00702     lasttime.tv_sec = header->ts.tv_sec;
00703     lasttime.tv_usec = header->ts.tv_usec;
00704 
00705     // The ETHERNET header is 42 bytes long; unnecessary
00706     enqueueHDLPacket(data + 42, header->len - 42);
00707 
00708     returnValue = pcap_next_ex(pcap, &header, &data);
00709   }
00710 }
00711 #endif //#ifdef HAVE_PCAP
00712 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:42