Beispiel #1
0
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;
 }