1. 概述
阅读Velodyne在ROS下VLP-16雷达节点源代码,将有用的部分总结一下,主要是时间戳以及topic的处理。
2. 溯源
雷达的节点源代码使用了nodelet这个概念,先慢慢看吧。根据源代码采用倒推的方式查找时间戳定义以及数据的读取代码。
1
构造函数 Convert::Convert中进行了节点初始化,主要就是从参数服务器中读取设定值。包括最小角度,最大角度等。
/** @brief Constructor. */
Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh, std::string const & node_name):
data_(new velodyne_rawdata::RawData()), first_rcfg_call(true),
diagnostics_(node, private_nh, node_name)
{
// Get startup parameters
private_nh.param<std::string>("fixed_frame", config_.fixed_frame, "velodyne");
private_nh.param<std::string>("target_Frame", config_.target_frame, "velodyne");
private_nh.param<double>("min_range", config_.min_range, 10.0);
private_nh.param<double>("max_range", config_.max_range, 200.0);
private_nh.param<bool>("organize_cloud", config_.organize_cloud, false);
boost::optional<velodyne_pointcloud::Calibration> calibration = data_->setup(private_nh);
if(calibration)
{
ROS_DEBUG_STREAM("Calibration file loaded.");
config_.num_lasers = static_cast<uint16_t>(calibration.get().num_lasers);
}
else
{
ROS_ERROR_STREAM("Could not load calibration file!");
}
if(config_.organize_cloud)
{
container_ptr_ = boost::shared_ptr<OrganizedCloudXYZIRT>(
new OrganizedCloudXYZIRT(config_.max_range, config_.min_range,
config_.target_frame, config_.fixed_frame,
config_.num_lasers, data_->scansPerPacket()));
}
else
{
container_ptr_ = boost::shared_ptr<PointcloudXYZIRT>(
new PointcloudXYZIRT(config_.max_range, config_.min_range,
config_.target_frame, config_.fixed_frame,
data_->scansPerPacket()));
}
// advertise output point cloud (before subscribing to input data)
output_ =
node.advertise<sensor_msgs::PointCloud2>("velodyne_points", 10);
srv_ = boost::make_shared <dynamic_reconfigure::Server<velodyne_pointcloud::
CloudNodeConfig> > (private_nh);
dynamic_reconfigure::Server<velodyne_pointcloud::CloudNodeConfig>::
CallbackType f;
f = boost::bind (&Convert::callback, this, _1, _2);
srv_->setCallback (f);
// subscribe to VelodyneScan packets
velodyne_scan_ =
node.subscribe("velodyne_packets", 10,
&Convert::processScan, (Convert *) this,
ros::TransportHints().tcpNoDelay(true));
// Diagnostics
diagnostics_.setHardwareID("Velodyne Convert");
// Arbitrary frequencies since we don't know which RPM is used, and are only
// concerned about monitoring the frequency.
diag_min_freq_ = 2.0;
diag_max_freq_ = 20.0;
using namespace diagnostic_updater;
diag_topic_.reset(new TopicDiagnostic("velodyne_points", diagnostics_,
FrequencyStatusParam(&diag_min_freq_,
&diag_max_freq_,
0.1, 10),
TimeStampStatusParam()));
}
需要说明
雷达的对数在校准文件中读取,我使用的是16通道,因此后面都按照num_lasers = 16来进行
boost::optional<velodyne_pointcloud::Calibration> calibration = data_->setup(private_nh);
if(calibration)
{
ROS_DEBUG_STREAM("Calibration file loaded.");
config_.num_lasers = static_cast<uint16_t>(calibration.get().num_lasers);
}
ROS下处理过后的雷达数据topic为velodyne_points,消息类型为sensor_msgs::PointCloud2
// advertise output point cloud (before subscribing to input data)
output_ =
node.advertise<sensor_msgs::PointCloud2>("velodyne_points"