void CraftUIApp::run() {
EventGenerator eventGen;
Cloud::Ptr capturedCloud;
/* calibrate the dynamic noise threshhold */
DynamicThreshCalib threshCalib(elementStorage->getElements());
for (int i=0; i<numThreshSamples; i++) {
updateCloud();
threshCalib.calibWithCloud(cloud);
if (withViewer) {
viewer->showCloud(cloud);
if (viewer->wasStopped()) return;
}
}
/* main loop */
while (!doQuit) {
updateCloud();
auto elements = elementStorage->getElements();
/* do all collisions */
for (auto element : elements) {
element->resetCollision();
element->collideCloud(cloud);
}
/* process events */
for (auto element : elements) {
element->accept(eventGen);
}
if (withViewer) {
viewer->showCloud(cloud);
if (viewer->wasStopped()) return;
}
}
}
void LaserScanVisualizer::cullPoints()
{
if ( point_decay_time_ == 0.0f )
{
return;
}
bool removed = false;
while ( !point_times_.empty() && point_times_.front() > point_decay_time_ )
{
point_times_.pop_front();
points_.pop_front();
cloud_messages_.pop_front();
removed = true;
}
if ( removed )
{
updateCloud();
}
}
void LaserScanVisualizer::transformCloud( std_msgs::PointCloudFloat32& message )
{
if ( point_decay_time_ == 0.0f )
{
points_.clear();
point_times_.clear();
cloud_messages_.clear();
}
// Push back before transforming. This will perform a full copy.
cloud_messages_.push_back( message );
if ( message.header.frame_id.empty() )
{
message.header.frame_id = target_frame_;
}
try
{
tf_client_->transformPointCloud(target_frame_, message, message);
}
catch(libTF::Exception& e)
{
printf( "Error transforming laser scan '%s', frame '%s' to frame '%s'\n", name_.c_str(), message.header.frame_id.c_str(), target_frame_.c_str() );
}
uint32_t point_count_ = message.get_pts_size();
for(uint32_t i = 0; i < point_count_; i++)
{
float& intensity = message.chan[0].vals[i];
// arbitrarily cap to 4096 for now
intensity = std::min( intensity, 4096.0f );
intensity_min_ = std::min( intensity_min_, intensity );
intensity_max_ = std::max( intensity_max_, intensity );
}
float diff_intensity = intensity_max_ - intensity_min_;
points_.push_back( V_Point() );
V_Point& points = points_.back();
points.resize( point_count_ );
point_times_.push_back( 0.0f );
for(uint32_t i = 0; i < point_count_; i++)
{
Ogre::Vector3 point( message.pts[i].x, message.pts[i].y, message.pts[i].z );
robotToOgre( point );
float intensity = message.chan[0].vals[i];
float normalized_intensity = (diff_intensity > 0.0f) ? ( intensity - intensity_min_ ) / diff_intensity : 1.0f;
Ogre::Vector3 color( r_, g_, b_ );
color *= normalized_intensity;
ogre_tools::PointCloud::Point& current_point = points[ i ];
current_point.x_ = point.x;
current_point.y_ = point.y;
current_point.z_ = point.z;
current_point.r_ = color.x;
current_point.g_ = color.y;
current_point.b_ = color.z;
}
updateCloud();
}
OpenNiInterface::Cloud::Ptr getNextCloud() {
updateCloud();
return cloud;
}