bool insert(T newData, float x, float y)
{
//std::cout << "["<< depth <<"] " << newData << " at " << x << " , " << y << " \n";
//Make sure point is within the bounds of this node
if (!isPointInRange(x, y, bounds))
{
//std::cout << "point not in bounds\n";
//Point is not within bounds
return false;
}
//Node isn't full yet, so add data to this node
if (!tL && data.size() < maxCapacity)
{
QuadPoint<T> newPoint;
newPoint.x = x;
newPoint.y = y;
newPoint.data = newData;
data.push_back(newPoint);
return true;
}
//Safety max depth
if (depth >= MAX_TREE_DEPTH)
{
std::cout << "\033[31mWARNING: Max tree depth (" << MAX_TREE_DEPTH << ") reached!\033[0m\n";
//return false;
}
//Node is full; subdivide (if not already subdivided)
if (!tL)
{
subdivide();
/*if (tL->subdivideInsert(newData, x, y)) return true;
if (tR->subdivideInsert(newData, x, y)) return true;
if (bL->subdivideInsert(newData, x, y)) return true;
if (bR->subdivideInsert(newData, x, y)) return true;
//Point wouldn't be accepted by any nodes
//std::cout <<"point rejected; keeping\n";
QuadPoint<T> newPoint;
newPoint.x = x;
newPoint.y = y;
newPoint.data = newData;
data.push_back(newPoint);
return true;*/
}
//If already subdivided, try inserting into child nodes
if (tL->insert(newData, x, y)) return true;
if (tR->insert(newData, x, y)) return true;
if (bL->insert(newData, x, y)) return true;
if (bR->insert(newData, x, y)) return true;
//Shouldn't ever happen
//std::cout << "This shouldn't happen\n";
return false;
}
//Fills the provided vector with resultant points (points
//contained in the specified range). Returns total results
unsigned int queryRange(aabb& range, std::vector<T>& results)
{
//std::cout << bounds.x << " , " << bounds.y << " w " << bounds.w << " h " << bounds.h << " \n";
unsigned int totalResults = 0;
//Make sure range is touching this node
if (!isColliding(&range, &bounds))
{
return 0;
}
//Add points in this node to results if contained in range
for (typename std::list<QuadPoint<T> >::iterator it = data.begin(); it!=data.end(); ++it)
{
//std::cout << "has point\n";
if (isPointInRange((*it).x, (*it).y, range))
{
results.push_back((*it).data);
totalResults++;
}
}
//Let all child nodes (if any) add points
if (!tL)
{
return totalResults;
}
totalResults += tL->queryRange(range, results);
totalResults += tR->queryRange(range, results);
totalResults += bL->queryRange(range, results);
totalResults += bR->queryRange(range, results);
return totalResults;
}
//Fills the provided array with resultant points (points
//contained in the specified range). Returns total results
//Pass in the maximum for the array as well as the index
//this function should start at (usually 0)
unsigned int queryRange(aabb& range, T* results, int& currentIndex, int maxPoints)
{
//std::cout << bounds.x << " , " << bounds.y << " w " << bounds.w << " h " << bounds.h << " \n";
unsigned int totalResults = 0;
//Make sure the array isn't full
if (currentIndex >= maxPoints)
{
std::cout << "WARNING: queryPoints(): Results array full! (Max points = "<< maxPoints<< ")\n";
return totalResults;
}
//Make sure range is touching this node
if (!isColliding(&range, &bounds))
{
return 0;
}
//Add points in this node to results if contained in range
for (typename std::list<QuadPoint<T> >::iterator it = data.begin(); it!=data.end(); ++it)
{
if (isPointInRange((*it).x, (*it).y, range))
{
if (currentIndex < maxPoints)
{
results[currentIndex] = (*it).data;
totalResults++;
currentIndex++;
}
else
{
std::cout << "WARNING: queryPoints(): Results array full! (Max points = "<< maxPoints<< ")\n";
return totalResults;
}
}
}
//Let all child nodes (if any) add points
if (!tL)
{
return totalResults;
}
totalResults += tL->queryRange(range, results, currentIndex, maxPoints);
totalResults += tR->queryRange(range, results, currentIndex, maxPoints);
totalResults += bL->queryRange(range, results, currentIndex, maxPoints);
totalResults += bR->queryRange(range, results, currentIndex, maxPoints);
return totalResults;
}
//Returns false if the point could not be removed (it doesn't exist)
//data is used to make sure it is the point requested
//NOTE: X and Y are not used when comparing the point, just for walking the tree,
//so if you have two of the same data, it might not remove the one
//at the position specified (the system assumes you are using pointers)
bool remove(T searchData, float x, float y)
{
//Point will not be in this node's bounds
if(!isPointInRange(x, y, bounds)) return false;
//Search through points for the data
for (typename std::list<QuadPoint<T> >::iterator it = data.begin(); it!= data.end(); ++it)
{
//Found the data point; erase it (inefficient only in large vectors)
//TODO: Replace vector with list or something?
if ((*it).data==searchData)
{
data.erase(it);
return true;
}
}
//Search children (if any)
if (!tL) return false;
if (tL->remove(searchData, x, y) || tR->remove(searchData, x, y)
|| bL->remove(searchData, x, y) || bR->remove(searchData, x, y))
{
//All children are empty; delete them
if (isEmpty())
{
//TODO: Should this be a pool?
delete tL;
delete tR;
delete bL;
delete bR;
tL = NULL;
}
return true;
}
//Data point not in quadtree
return false;
}
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;
}
