driver.cc
Go to the documentation of this file.
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$
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


velodyne_driver
Author(s): Jack O'Quin, Patrick Beeson, Michael Quinlan, Yaxin Liu
autogenerated on Fri Jan 3 2014 12:11:08