49 {
50
51 out_msg->mutable_header()->set_timestamp_sec(
52 scan_msg->basetime() / 1000000000.0);
53 out_msg->mutable_header()->set_module_name(
54 scan_msg->header().module_name());
55 out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id());
56 out_msg->set_height(1);
57 out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0);
58 out_msg->mutable_header()->set_sequence_num(
59 scan_msg->header().sequence_num());
60 gps_base_usec_ = scan_msg->basetime();
61
62 frame_count++;
63 const unsigned char *difop_ptr
64 = (const unsigned char *)scan_msg->difop_pkts(0).data().c_str();
65
66 if (difop_ptr[0] == 0x00 || difop_ptr[0] == 0xa5) {
67 if (difop_ptr[1] == 0xff && difop_ptr[2] == 0x00
68 && difop_ptr[3] == 0x5a) {
69 if (difop_ptr[231] == 64 || difop_ptr[231] == 65) {
70 is_add_frame_ = true;
71 }
72
73 int majorVersion = difop_ptr[1202];
74 int minorVersion1 = difop_ptr[1203] / 16;
75
76
77 if (1 > majorVersion || (1 == majorVersion && minorVersion1 > 1)) {
78 g_fAngleAcc_V = 0.0025;
79 } else {
80 g_fAngleAcc_V = 0.01;
81 }
82
83 float fInitAngle_V = difop_ptr[188] * 256 + difop_ptr[189];
84 if (fInitAngle_V > 32767) {
85 fInitAngle_V = fInitAngle_V - 65536;
86 }
87 this->
prism_angle[0] = fInitAngle_V * g_fAngleAcc_V;
88
89 fInitAngle_V = difop_ptr[190] * 256 + difop_ptr[191];
90 if (fInitAngle_V > 32767) {
91 fInitAngle_V = fInitAngle_V - 65536;
92 }
93 this->
prism_angle[1] = fInitAngle_V * g_fAngleAcc_V;
94
95 fInitAngle_V = difop_ptr[192] * 256 + difop_ptr[193];
96 if (fInitAngle_V > 32767) {
97 fInitAngle_V = fInitAngle_V - 65536;
98 }
99 this->
prism_angle[2] = fInitAngle_V * g_fAngleAcc_V;
100
101 fInitAngle_V = difop_ptr[194] * 256 + difop_ptr[195];
102 if (fInitAngle_V > 32767) {
103 fInitAngle_V = fInitAngle_V - 65536;
104 }
105 this->
prism_angle[3] = fInitAngle_V * g_fAngleAcc_V;
106 }
107 }
108
109 packets_size = scan_msg->firing_pkts_size();
110 packet_number_ = packets_size;
111
112 for (size_t i = 0; i < packets_size; ++i) {
113 Unpack(static_cast<int>(i),
114 scan_msg->firing_pkts(static_cast<int>(i)),
115 out_msg);
116 }
117
118 if (is_add_frame_) {
119 if (frame_count >= 2) {
120
121 for (int j = 0; j < cur_pc->point_size(); ++j) {
122 PointXYZIT *point3 = out_msg->add_point();
123 point3->set_timestamp(cur_pc->point(j).timestamp());
124 point3->set_intensity(cur_pc->point(j).intensity());
125 point3->set_x(cur_pc->point(j).x());
126 point3->set_y(cur_pc->point(j).y());
127 point3->set_z(cur_pc->point(j).z());
128 }
129 }
130 cur_pc = pre_pc;
132 } else {
133
134 for (int j = 0; j < cur_pc->point_size(); ++j) {
135 PointXYZIT *point3 = out_msg->add_point();
136 point3->set_timestamp(cur_pc->point(j).timestamp());
137 point3->set_intensity(cur_pc->point(j).intensity());
138 point3->set_x(cur_pc->point(j).x());
139 point3->set_y(cur_pc->point(j).y());
140 point3->set_z(cur_pc->point(j).z());
141 }
144 }
145 AINFO <<
"line: " << __LINE__ <<
"out_msg size: " << out_msg->point_size();
146 AINFO <<
"packets_size :" << packets_size;
147 if (out_msg->point().empty()) {
148
150 }
151
152
153 out_msg->set_width(out_msg->point_size());
154 out_msg->set_height(1);
155}