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;
}
