$search
00001 /* 00002 * Copyright (C) 2007 Austin Robot Technology, Patrick Beeson 00003 * Copyright (C) 2009-2012 Austin Robot Technology, Jack O'Quin 00004 * 00005 * License: Modified BSD Software License Agreement 00006 * 00007 * $Id: driver.cc 2221 2012-03-25 23:12:32Z jack.oquin $ 00008 */ 00009 00015 #include <string> 00016 #include <cmath> 00017 00018 #include <ros/ros.h> 00019 #include <tf/transform_listener.h> 00020 #include <velodyne_msgs/VelodyneScan.h> 00021 00022 #include "driver.h" 00023 00024 namespace velodyne_driver 00025 { 00026 00027 VelodyneDriver::VelodyneDriver(ros::NodeHandle node, 00028 ros::NodeHandle private_nh) 00029 { 00030 // use private node handle to get parameters 00031 private_nh.param("frame_id", config_.frame_id, std::string("velodyne")); 00032 std::string tf_prefix = tf::getPrefixParam(private_nh); 00033 ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix); 00034 config_.frame_id = tf::resolve(tf_prefix, config_.frame_id); 00035 00036 // get model name, validate string, determine packet rate 00037 private_nh.param("model", config_.model, std::string("64E")); 00038 double packet_rate; // packet frequency (Hz) 00039 if (config_.model == "64E") 00040 { 00041 packet_rate = 2600.0; 00042 } 00043 else if (config_.model == "32E") 00044 { 00045 packet_rate = 1808.0; 00046 } 00047 else 00048 { 00049 ROS_ERROR_STREAM("unknown Velodyne LIDAR model: " << config_.model); 00050 packet_rate = 2600.0; 00051 } 00052 std::string deviceName("Velodyne HDL-" + config_.model); 00053 00054 private_nh.param("rpm", config_.rpm, 600.0); 00055 ROS_INFO_STREAM(deviceName << " rotating at " << config_.rpm << " RPM"); 00056 double frequency = (config_.rpm / 60.0); // expected Hz rate 00057 00058 // default number of packets for each scan is a single revolution 00059 // (fractions rounded up) 00060 config_.npackets = (int) ceil(packet_rate / frequency); 00061 private_nh.getParam("npackets", config_.npackets); 00062 ROS_INFO_STREAM("publishing " << config_.npackets << " packets per scan"); 00063 00064 std::string dump_file; 00065 private_nh.param("pcap", dump_file, std::string("")); 00066 00067 // initialize diagnostics 00068 diagnostics_.setHardwareID(deviceName); 00069 diag_max_freq_ = frequency; 00070 diag_min_freq_ = frequency; 00071 ROS_INFO("expected frequency: %.3f (Hz)", frequency); 00072 00073 using namespace diagnostic_updater; 00074 diag_topic_.reset(new TopicDiagnostic("velodyne_packets", diagnostics_, 00075 FrequencyStatusParam(&diag_min_freq_, 00076 &diag_max_freq_, 00077 0.1, 10), 00078 TimeStampStatusParam())); 00079 00080 // open Velodyne input device or file 00081 if (dump_file != "") 00082 { 00083 input_.reset(new velodyne_driver::InputPCAP(private_nh, 00084 packet_rate, 00085 dump_file)); 00086 } 00087 else 00088 { 00089 input_.reset(new velodyne_driver::InputSocket(private_nh)); 00090 } 00091 00092 // raw data output topic 00093 output_ = node.advertise<velodyne_msgs::VelodyneScan>("velodyne_packets", 10); 00094 } 00095 00100 bool VelodyneDriver::poll(void) 00101 { 00102 // Allocate a new shared pointer for zero-copy sharing with other nodelets. 00103 velodyne_msgs::VelodyneScanPtr scan(new velodyne_msgs::VelodyneScan); 00104 scan->packets.resize(config_.npackets); 00105 00106 // Since the velodyne delivers data at a very high rate, keep 00107 // reading and publishing scans as fast as possible. 00108 for (int i = 0; i < config_.npackets; ++i) 00109 { 00110 while (true) 00111 { 00112 // keep reading until full packet received 00113 int rc = input_->getPacket(&scan->packets[i]); 00114 if (rc == 0) break; // got a full packet? 00115 if (rc < 0) return false; // end of file reached? 00116 } 00117 } 00118 00119 // publish message using time of last packet read 00120 ROS_DEBUG("Publishing a full Velodyne scan."); 00121 scan->header.stamp = ros::Time(scan->packets[config_.npackets - 1].stamp); 00122 scan->header.frame_id = config_.frame_id; 00123 output_.publish(scan); 00124 00125 // notify diagnostics that a message has been published, updating 00126 // its status 00127 diag_topic_->tick(scan->header.stamp); 00128 diagnostics_.update(); 00129 00130 return true; 00131 } 00132 00133 } // namespace velodyne_driver