31 {
32 CHECK_NOTNULL(lidar_frame);
33
34 if (msg.height() > 1 && msg.width() > 1) {
35 for (unsigned int i = 0; i < msg.height(); ++i) {
36 for (unsigned int j = 0; j < msg.width(); ++j) {
37 Eigen::Vector3d pt3d;
38 pt3d[0] = static_cast<double>(msg.point(i * msg.width() + j).x());
39 pt3d[1] = static_cast<double>(msg.point(i * msg.width() + j).y());
40 pt3d[2] = static_cast<double>(msg.point(i * msg.width() + j).z());
41 if (!std::isnan(pt3d[0])) {
42 Eigen::Vector3d pt3d_tem = pt3d;
43
45 continue;
46 }
47 unsigned char intensity = static_cast<unsigned char>(
48 msg.point(i * msg.width() + j).intensity());
49 lidar_frame->pt_xs.push_back(pt3d[0]);
50 lidar_frame->pt_ys.push_back(pt3d[1]);
51 lidar_frame->pt_zs.push_back(pt3d[2]);
52 lidar_frame->intensities.push_back(intensity);
53 }
54 }
55 }
56 } else {
57 AINFO <<
"Receiving un-organized-point-cloud, width " << msg.width()
58 << " height " << msg.height() << "size " << msg.point_size();
59 for (int i = 0; i < msg.point_size(); ++i) {
60 Eigen::Vector3d pt3d;
61 pt3d[0] = static_cast<double>(msg.point(i).x());
62 pt3d[1] = static_cast<double>(msg.point(i).y());
63 pt3d[2] = static_cast<double>(msg.point(i).z());
64 if (!std::isnan(pt3d[0])) {
65 Eigen::Vector3d pt3d_tem = pt3d;
66
68 continue;
69 }
70 unsigned char intensity =
71 static_cast<unsigned char>(msg.point(i).intensity());
72 lidar_frame->pt_xs.push_back(pt3d[0]);
73 lidar_frame->pt_ys.push_back(pt3d[1]);
74 lidar_frame->pt_zs.push_back(pt3d[2]);
75 lidar_frame->intensities.push_back(intensity);
76 }
77 }
78 }
79
80 lidar_frame->measurement_time =
81 cyber::Time(msg.measurement_time()).ToSecond();
82 if (FLAGS_lidar_debug_log_flag) {
83 AINFO << std::setprecision(15) <<
"LocalLidar Debug Log: velodyne msg. "
84 << "[time:" << lidar_frame->measurement_time
85 << "][height:" << msg.height() << "][width:" << msg.width()
86 << "][point_cnt:" << msg.point_size() << "]";
87 }
88}