Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
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
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
00037 private_nh.param("model", config_.model, std::string("64E"));
00038 double packet_rate;
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);
00057
00058
00059
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
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
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
00093 output_ = node.advertise<velodyne_msgs::VelodyneScan>("velodyne_packets", 10);
00094 }
00095
00100 bool VelodyneDriver::poll(void)
00101 {
00102
00103 velodyne_msgs::VelodyneScanPtr scan(new velodyne_msgs::VelodyneScan);
00104 scan->packets.resize(config_.npackets);
00105
00106
00107
00108 for (int i = 0; i < config_.npackets; ++i)
00109 {
00110 while (true)
00111 {
00112
00113 int rc = input_->getPacket(&scan->packets[i]);
00114 if (rc == 0) break;
00115 if (rc < 0) return false;
00116 }
00117 }
00118
00119
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
00126
00127 diag_topic_->tick(scan->header.stamp);
00128 diagnostics_.update();
00129
00130 return true;
00131 }
00132
00133 }