viewList_t LLViewQuery::run(LLView * view) const
{
viewList_t result;
filterResult_t pre = runFilters(view, viewList_t(), mPreFilters);
if(!pre.first && !pre.second)
{
// skip post filters completely if we're not including ourselves or the children
return result;
}
if(pre.second)
{
// run filters on children
viewList_t filtered_children;
filterChildren(view, filtered_children);
filterResult_t post = runFilters(view, filtered_children, mPostFilters);
if(pre.first && post.first)
{
result.push_back(view);
}
if(post.second)
{
result.insert(result.end(), filtered_children.begin(), filtered_children.end());
}
}
else
{
if(pre.first)
{
result.push_back(view);
}
}
return result;
}
void processData(void)
{
/* remove old data */
ros::Time &time = m_toProcess.header.stamp;
m_worldDataMutex.lock();
while (!m_currentWorld.empty() && (time - m_currentWorld.front()->header.stamp).to_double() > m_retainPointcloudDuration)
{
std_msgs::PointCloudFloat32* old = m_currentWorld.front();
m_currentWorld.pop_front();
delete old;
}
/* add new data */
std_msgs::PointCloudFloat32 *newData = runFilters(m_toProcess);
if (newData)
{
if (newData->get_pts_size() == 0)
delete newData;
else
m_currentWorld.push_back(newData);
}
/* let the publishing thread know there is new data to publish */
m_shouldPublish = true;
if (m_verbose)
{
double window = m_currentWorld.size() > 0 ? (m_currentWorld.back()->header.stamp - m_currentWorld.front()->header.stamp).to_double() : 0.0;
printf("World map containing %d point clouds (window size = %f seconds)\n", m_currentWorld.size(), window);
}
m_worldDataMutex.unlock();
}
viewList_t LLViewQuery::run(LLView* view) const
{
viewList_t result;
viewList_t const child_list(view->getChildList()->begin(), view->getChildList()->end());
// prefilter gets immediate children of view
filterResult_t pre = runFilters(view, child_list, mPreFilters);
if(!pre.first && !pre.second)
{
// not including ourselves or the children
// nothing more to do
return result;
}
viewList_t filtered_children;
filterResult_t post(TRUE, TRUE);
if(pre.second)
{
// run filters on children
filterChildren(view, filtered_children);
// only run post filters if this element passed pre filters
// so if you failed to pass the pre filter, you can't filter out children in post
if (pre.first)
{
post = runFilters(view, filtered_children, mPostFilters);
}
}
if(pre.first && post.first)
{
result.push_back(view);
}
if(pre.second && post.second)
{
result.insert(result.end(), filtered_children.begin(), filtered_children.end());
}
return result;
}
int main(int argc, char **argv) {
tagBITMAPFILEHEADER BITMAPFILEHEADER;
tagBITMAPINFOHEADER BITMAPINFOHEADER;
matrix image;
char source_file_name[100];
parameters inputParameters;
bool dontStopMeNow = true;//flag for help calling or incorrect parameters
initParameters(inputParameters);
readParameters(argc, argv, inputParameters, source_file_name[0], dontStopMeNow);
//function: sort parameters by queue --> user can combine it as he want
if (dontStopMeNow) {
readBMP(BITMAPFILEHEADER, BITMAPINFOHEADER, image, source_file_name[0]);
runFilters(inputParameters, BITMAPFILEHEADER, BITMAPINFOHEADER, image);
writeBMP(BITMAPFILEHEADER, BITMAPINFOHEADER, image);
}
}
void processData(const std_msgs::PointCloud& local_cloud)
{
if (!m_acceptScans){
ROS_INFO("Rejecting scans\n");
return;
}
m_worldDataMutex.lock();
point_clouds_.push_back(local_cloud);
while(!point_clouds_.empty()){
const std_msgs::PointCloud& point_cloud = point_clouds_.front();
std_msgs::PointCloud map_cloud;
/* Transform to the map frame */
try
{
m_tf.transformPointCloud("map", map_cloud, point_cloud);
}
catch(libTF::TransformReference::LookupException& ex)
{
ROS_ERROR("Lookup exception: %s\n", ex.what());
break;
}
catch(libTF::TransformReference::ExtrapolateException& ex)
{
ROS_ERROR("Extrapolation exception: %s\n", ex.what());
break;
}
catch(libTF::TransformReference::ConnectivityException& ex)
{
ROS_ERROR("Connectivity exception: %s\n", ex.what());
break;
}
catch(...)
{
ROS_ERROR("Exception in point cloud computation\n");
break;
}
point_clouds_.pop_front();
/* add new data */
ROS_DEBUG( "Received laser scan with %d points in frame %s\n",
local_cloud.get_pts_size(), local_cloud.header.frame_id.c_str());
std_msgs::PointCloud *newData = runFilters(map_cloud);
if (newData){
if (newData->get_pts_size() == 0)
delete newData;
else{
m_currentWorld.push_back(newData);
}
}
}
m_worldDataMutex.unlock();
}
