void VelodynePostNode::publish() {
if (_pointCloudPublisher.getNumSubscribers() == 0)
return;
VdynePointCloud pointCloud;
for (auto it = _dataPackets.cbegin(); it != _dataPackets.cend(); ++it)
Converter::toPointCloud(*it, *_calibration, pointCloud, _minDistance,
_maxDistance);
auto rosPointCloud = boost::make_shared<sensor_msgs::PointCloud>();
rosPointCloud->header.stamp =
ros::Time().fromNSec(_dataPackets.front().getTimestamp()
+ std::round((_dataPackets.back().getTimestamp() -
_dataPackets.front().getTimestamp()) * 0.5));
rosPointCloud->header.frame_id = _frameId;
const auto numPoints = pointCloud.getSize();
rosPointCloud->points.reserve(numPoints);
rosPointCloud->channels.resize(1);
rosPointCloud->channels[0].name = "intensity";
rosPointCloud->channels[0].values.reserve(numPoints);
const auto& points = pointCloud.getPoints();
for (const auto& point : points) {
geometry_msgs::Point32 rosPoint;
rosPoint.x = point.mX;
rosPoint.y = point.mY;
rosPoint.z = point.mZ;
rosPointCloud->points.push_back(rosPoint);
rosPointCloud->channels[0].values.push_back(point.mIntensity);
}
auto rosPointCloud2 = boost::make_shared<sensor_msgs::PointCloud2>();
convertPointCloudToPointCloud2 (*rosPointCloud, *rosPointCloud2);
_pointCloudPublisher.publish(rosPointCloud2);
}
int main(int argc, char *argv[]) {
UDPConnectionServer connection(2368);
AcquisitionThread<DataPacket> thread(connection);
Calibration calib;
std::shared_ptr<DataPacket> packet;
IPC_RETURN_TYPE err;
char dump_filename[4096];
carmen_velodyne_ipc_initialize(argc, argv);
carmen_velodyne_read_parameters(argc, argv);
carmen_velodyne_set_spin_rate(spin_rate);
calib_file.open(calib_path);
if (!calib_file.is_open())
carmen_die("\nError: Could not open %s for reading\n", calib_filename);
calib_file >> calib;
calib_file.close();
signal(SIGINT, carmen_velodyne_sigint_handler);
thread.start();
double time;
double start_time = carmen_get_time();
int num_packets = 0;
int start_num_packets = 0;
int num_lost_packets = 0;
int start_num_lost_packets = 0;
while (!quit) {
try {
while (dump_enabled || points_publish) {
packet = thread.getBuffer().dequeue();
++num_packets;
if (dump_enabled) {
if (!dump_file.is_open()) {
char dump_time[1024];
time_t local = packet->getTimestamp();
struct tm* time = localtime(&local);
strftime(dump_time, sizeof(dump_time), "%Y-%m-%d-%H%M%S", time);
sprintf(dump_filename, "%s/%s.bin", dump_path, dump_time);
dump_file.open(dump_filename, std::ios::out | std::ios::binary);
}
if (dump_file.is_open()) {
long dump_filepos = dump_file.tellp();
dump_file << *packet;
carmen_velodyne_publish_packet(id, dump_filename,
dump_filepos, packet->getTimestamp());
}
else {
carmen_warn("\nWarning: Could not open %s for writing\n",
dump_filename);
}
}
if (points_publish) {
VdynePointCloud pointCloud;
Converter::toPointCloud(*packet, calib, pointCloud);
int num_points = pointCloud.getSize();
float x[num_points], y[num_points], z[num_points];
std::vector<VdynePointCloud::Point3D>::const_iterator it;
int i = 0;
for (it = pointCloud.getPointBegin();
it != pointCloud.getPointEnd(); ++it, ++i) {
x[i] = it->mX;
y[i] = it->mY;
z[i] = it->mZ;
}
carmen_velodyne_publish_pointcloud(id, num_points, x, y, z,
packet->getTimestamp());
}
double time = carmen_get_time();
if (time-start_time >= 1.0) {
num_lost_packets = thread.getBuffer().getNumDroppedElements();
float period_num_packets = num_packets-start_num_packets;
float period_num_lost_packets = num_lost_packets-
start_num_lost_packets;
double packet_loss = period_num_lost_packets/
(period_num_packets+period_num_lost_packets);
fprintf(stdout, "\rPacket loss: %6.2f%%", packet_loss*100.0);
fflush(stdout);
start_time = time;
start_num_packets = num_packets;
start_num_lost_packets = num_lost_packets;
}
}
}
catch (IOException& exception) {
// buffer underrun
}
carmen_ipc_sleep(0.0);
}
if (dump_file.is_open())
dump_file.close();
fprintf(stdout, "\n");
return 0;
}
