00001 /* 00002 * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin 00003 * 00004 * License: Modified BSD Software License Agreement 00005 * 00006 * $Id$ 00007 */ 00008 00014 #include <string> 00015 #include <boost/thread.hpp> 00016 00017 #include <ros/ros.h> 00018 #include <pluginlib/class_list_macros.h> 00019 #include <nodelet/nodelet.h> 00020 00021 #include "driver.h" 00022 00023 namespace velodyne_driver 00024 { 00025 00026 class DriverNodelet: public nodelet::Nodelet 00027 { 00028 public: 00029 00030 DriverNodelet(): 00031 running_(false) 00032 {} 00033 00034 ~DriverNodelet() 00035 { 00036 if (running_) 00037 { 00038 NODELET_INFO("shutting down driver thread"); 00039 running_ = false; 00040 deviceThread_->join(); 00041 NODELET_INFO("driver thread stopped"); 00042 } 00043 } 00044 00045 private: 00046 00047 virtual void onInit(void); 00048 virtual void devicePoll(void); 00049 00050 volatile bool running_; 00051 boost::shared_ptr<boost::thread> deviceThread_; 00052 00053 boost::shared_ptr<VelodyneDriver> dvr_; 00054 }; 00055 00056 void DriverNodelet::onInit() 00057 { 00058 // start the driver 00059 dvr_.reset(new VelodyneDriver(getNodeHandle(), getPrivateNodeHandle())); 00060 00061 // spawn device poll thread 00062 running_ = true; 00063 deviceThread_ = boost::shared_ptr< boost::thread > 00064 (new boost::thread(boost::bind(&DriverNodelet::devicePoll, this))); 00065 } 00066 00068 void DriverNodelet::devicePoll() 00069 { 00070 while(ros::ok()) 00071 { 00072 // poll device until end of file 00073 running_ = dvr_->poll(); 00074 if (!running_) 00075 break; 00076 } 00077 running_ = false; 00078 } 00079 00080 } // namespace velodyne_driver 00081 00082 // Register this plugin with pluginlib. Names must match nodelet_velodyne.xml. 00083 // 00084 // parameters are: package, class name, class type, base class type 00085 PLUGINLIB_DECLARE_CLASS(velodyne_driver, DriverNodelet, 00086 velodyne_driver::DriverNodelet, nodelet::Nodelet);