void Scanner2d::processMsg() { uint8_t *payload = &(msg_->buffer[SCANNER2D_MSG_HEADER_LEN]); switch (msg_->msg_type) { case 0: publishScan(payload, msg_->payload_length); break; case 1: updateStatus(payload); break; } }
void LslidarN301Decoder::packetCallback( const lslidar_n301_msgs::LslidarN301PacketConstPtr& msg) { // ROS_WARN("packetCallBack"); // Convert the msg to the raw packet type. const RawPacket* raw_packet = (const RawPacket*) (&(msg->data[0])); // Check if the packet is valid if (!checkPacketValidity(raw_packet)) return; // Decode the packet decodePacket(raw_packet); // Find the start of a new revolution // If there is one, new_sweep_start will be the index of the start firing, // otherwise, new_sweep_start will be FIRINGS_PER_PACKET. size_t new_sweep_start = 0; do { // if (firings[new_sweep_start].firing_azimuth < last_azimuth) break; if (fabs(firings[new_sweep_start].firing_azimuth - last_azimuth) > M_PI) break; else { last_azimuth = firings[new_sweep_start].firing_azimuth; ++new_sweep_start; } } while (new_sweep_start < FIRINGS_PER_PACKET); // ROS_WARN("new_sweep_start %d", new_sweep_start); // The first sweep may not be complete. So, the firings with // the first sweep will be discarded. We will wait for the // second sweep in order to find the 0 azimuth angle. size_t start_fir_idx = 0; size_t end_fir_idx = new_sweep_start; if (is_first_sweep && new_sweep_start == FIRINGS_PER_PACKET) { // The first sweep has not ended yet. return; } else { if (is_first_sweep) { is_first_sweep = false; start_fir_idx = new_sweep_start; end_fir_idx = FIRINGS_PER_PACKET; sweep_start_time = msg->stamp.toSec() + FIRING_TOFFSET * (end_fir_idx-start_fir_idx) * 1e-6; } } for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) { for (size_t scan_idx = 0; scan_idx < SCANS_PER_FIRING; ++scan_idx) { // Check if the point is valid. if (!isPointInRange(firings[fir_idx].distance[scan_idx])) continue; // Convert the point to xyz coordinate size_t table_idx = floor(firings[fir_idx].azimuth[scan_idx]*1000.0+0.5); //cout << table_idx << endl; double cos_azimuth = cos_azimuth_table[table_idx]; double sin_azimuth = sin_azimuth_table[table_idx]; //double x = firings[fir_idx].distance[scan_idx] * // cos_scan_altitude[scan_idx] * sin(firings[fir_idx].azimuth[scan_idx]); //double y = firings[fir_idx].distance[scan_idx] * // cos_scan_altitude[scan_idx] * cos(firings[fir_idx].azimuth[scan_idx]); //double z = firings[fir_idx].distance[scan_idx] * // sin_scan_altitude[scan_idx]; double x = firings[fir_idx].distance[scan_idx] * cos_scan_altitude[scan_idx] * sin_azimuth; double y = firings[fir_idx].distance[scan_idx] * cos_scan_altitude[scan_idx] * cos_azimuth; double z = firings[fir_idx].distance[scan_idx] * sin_scan_altitude[scan_idx]; double x_coord = y; double y_coord = -x; double z_coord = z; // Compute the time of the point double time = packet_start_time + FIRING_TOFFSET*fir_idx + DSR_TOFFSET*scan_idx; // Remap the index of the scan int remapped_scan_idx = scan_idx%2 == 0 ? scan_idx/2 : scan_idx/2+8; sweep_data->scans[remapped_scan_idx].points.push_back( lslidar_n301_msgs::LslidarN301Point()); lslidar_n301_msgs::LslidarN301Point& new_point = // new_point 为push_back最后一个的引用 sweep_data->scans[remapped_scan_idx].points[ sweep_data->scans[remapped_scan_idx].points.size()-1]; // Pack the data into point msg new_point.time = time; new_point.x = x_coord; new_point.y = y_coord; new_point.z = z_coord; new_point.azimuth = firings[fir_idx].azimuth[scan_idx]; new_point.distance = firings[fir_idx].distance[scan_idx]; new_point.intensity = firings[fir_idx].intensity[scan_idx]; } } packet_start_time += FIRING_TOFFSET * (end_fir_idx-start_fir_idx); // A new sweep begins if (end_fir_idx != FIRINGS_PER_PACKET) { // ROS_WARN("A new sweep begins"); // Publish the last revolution //sweep_data->header.stamp = ros::Time(sweep_start_time); sweep_data->header.stamp = ros::Time(); sweep_pub.publish(sweep_data); if (publish_point_cloud) publishPointCloud(); publishScan(); sweep_data = lslidar_n301_msgs::LslidarN301SweepPtr( new lslidar_n301_msgs::LslidarN301Sweep()); // Prepare the next revolution sweep_start_time = msg->stamp.toSec() + FIRING_TOFFSET * (end_fir_idx-start_fir_idx) * 1e-6; packet_start_time = 0.0; last_azimuth = firings[FIRINGS_PER_PACKET-1].firing_azimuth; start_fir_idx = end_fir_idx; end_fir_idx = FIRINGS_PER_PACKET; for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) { for (size_t scan_idx = 0; scan_idx < SCANS_PER_FIRING; ++scan_idx) { // Check if the point is valid. if (!isPointInRange(firings[fir_idx].distance[scan_idx])) continue; // Convert the point to xyz coordinate size_t table_idx = floor(firings[fir_idx].azimuth[scan_idx]*1000.0+0.5); //cout << table_idx << endl; double cos_azimuth = cos_azimuth_table[table_idx]; double sin_azimuth = sin_azimuth_table[table_idx]; //double x = firings[fir_idx].distance[scan_idx] * // cos_scan_altitude[scan_idx] * sin(firings[fir_idx].azimuth[scan_idx]); //double y = firings[fir_idx].distance[scan_idx] * // cos_scan_altitude[scan_idx] * cos(firings[fir_idx].azimuth[scan_idx]); //double z = firings[fir_idx].distance[scan_idx] * // sin_scan_altitude[scan_idx]; double x = firings[fir_idx].distance[scan_idx] * cos_scan_altitude[scan_idx] * sin_azimuth; double y = firings[fir_idx].distance[scan_idx] * cos_scan_altitude[scan_idx] * cos_azimuth; double z = firings[fir_idx].distance[scan_idx] * sin_scan_altitude[scan_idx]; double x_coord = y; double y_coord = -x; double z_coord = z; // Compute the time of the point double time = packet_start_time + FIRING_TOFFSET*(fir_idx-start_fir_idx) + DSR_TOFFSET*scan_idx; // Remap the index of the scan int remapped_scan_idx = scan_idx%2 == 0 ? scan_idx/2 : scan_idx/2+8; sweep_data->scans[remapped_scan_idx].points.push_back( lslidar_n301_msgs::LslidarN301Point()); lslidar_n301_msgs::LslidarN301Point& new_point = sweep_data->scans[remapped_scan_idx].points[ sweep_data->scans[remapped_scan_idx].points.size()-1]; // Pack the data into point msg new_point.time = time; new_point.x = x_coord; new_point.y = y_coord; new_point.z = z_coord; new_point.azimuth = firings[fir_idx].azimuth[scan_idx]; new_point.distance = firings[fir_idx].distance[scan_idx]; new_point.intensity = firings[fir_idx].intensity[scan_idx]; } } packet_start_time += FIRING_TOFFSET * (end_fir_idx-start_fir_idx); } // ROS_WARN("pack end"); return; }