void GridCellsDisplay::processMessage(const nav_msgs::GridCells::ConstPtr& msg) { if (!msg) { return; } cloud_->clear(); ++messages_received_; if (!validateFloats(*msg)) { setStatus("Topic", eu::nifti::ocu::STATUS_LEVEL_ERROR, "Message contained invalid floating point values (nans or infs)"); return; } std::stringstream ss; ss << messages_received_ << " messages received"; setStatus("Topic", eu::nifti::ocu::STATUS_LEVEL_OK, ss.str()); Ogre::Vector3 position; Ogre::Quaternion orientation; if (!frameTransformer->getTransform(msg->header, position, orientation)) { ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), fixed_frame_.c_str() ); } scene_node_->setPosition( position ); scene_node_->setOrientation( orientation ); cloud_->setDimensions(msg->cell_width, msg->cell_height, 0.0); uint32_t num_points = msg->cells.size(); typedef std::vector< ogre_tools::PointCloud::Point > V_Point; V_Point points; points.resize( num_points ); for(uint32_t i = 0; i < num_points; i++) { ogre_tools::PointCloud::Point& current_point = points[ i ]; Ogre::Vector3 pos(msg->cells[i].x, msg->cells[i].y, msg->cells[i].z); current_point.x = pos.x; current_point.y = pos.y; current_point.z = pos.z; current_point.setColor(color.r, color.g, color.b); } cloud_->clear(); if ( !points.empty() ) { cloud_->addPoints( &points.front(), points.size() ); } }
void PointCloudDisplay::transformCloud() { if ( message_.header.frame_id.empty() ) { message_.header.frame_id = fixed_frame_; } tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0, 0, 0 ), btVector3( 0, 0, 0 ) ), message_.header.stamp, message_.header.frame_id ); try { tf_->transformPose( fixed_frame_, pose, pose ); } catch(tf::TransformException& e) { ROS_ERROR( "Error transforming point cloud '%s' from frame '%s' to frame '%s'\n", name_.c_str(), message_.header.frame_id.c_str(), fixed_frame_.c_str() ); } Ogre::Vector3 position( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() ); robotToOgre( position ); btScalar yaw, pitch, roll; pose.getBasis().getEulerZYX( yaw, pitch, roll ); Ogre::Matrix3 orientation( ogreMatrixFromRobotEulers( yaw, pitch, roll ) ); // First find the min/max intensity values float min_intensity = 999999.0f; float max_intensity = -999999.0f; typedef std::vector<std_msgs::ChannelFloat32> V_Chan; typedef std::vector<bool> V_bool; V_bool valid_channels(message_.chan.size()); uint32_t point_count = message_.get_pts_size(); V_Chan::iterator chan_it = message_.chan.begin(); V_Chan::iterator chan_end = message_.chan.end(); uint32_t index = 0; for ( ; chan_it != chan_end; ++chan_it, ++index ) { std_msgs::ChannelFloat32& chan = *chan_it; uint32_t val_count = chan.vals.size(); bool channel_size_correct = val_count == point_count; ROS_ERROR_COND(!channel_size_correct, "Point cloud '%s' on topic '%s' has channel 0 with fewer values than points (%d values, %d points)", name_.c_str(), topic_.c_str(), val_count, point_count); valid_channels[index] = channel_size_correct; if ( channel_size_correct && ( chan.name.empty() || chan.name == "intensity" || chan.name == "intensities" ) ) { for(uint32_t i = 0; i < point_count; i++) { float& intensity = chan.vals[i]; // arbitrarily cap to 4096 for now intensity = std::min( intensity, 4096.0f ); min_intensity = std::min( min_intensity, intensity ); max_intensity = std::max( max_intensity, intensity ); } } } float diff_intensity = max_intensity - min_intensity; typedef std::vector< ogre_tools::PointCloud::Point > V_Point; V_Point points; points.resize( point_count ); for(uint32_t i = 0; i < point_count; i++) { Ogre::Vector3 color( color_.r_, color_.g_, color_.b_ ); ogre_tools::PointCloud::Point& current_point = points[ i ]; current_point.x_ = message_.pts[i].x; current_point.y_ = message_.pts[i].y; current_point.z_ = message_.pts[i].z; current_point.r_ = color.x; current_point.g_ = color.y; current_point.b_ = color.z; } chan_it = message_.chan.begin(); index = 0; for ( ; chan_it != chan_end; ++chan_it, ++index ) { if ( !valid_channels[index] ) { continue; } std_msgs::ChannelFloat32& chan = *chan_it; enum ChannelType { CT_INTENSITY, CT_RGB, CT_R, CT_G, CT_B, CT_COUNT }; ChannelType type = CT_INTENSITY; if ( chan.name == "rgb" ) { type = CT_RGB; } else if ( chan.name == "r" ) { type = CT_R; } else if ( chan.name == "g" ) { type = CT_G; } else if ( chan.name == "b" ) { type = CT_B; } typedef void (*TransformFunc)(float, ogre_tools::PointCloud::Point&, float, float, float); TransformFunc funcs[CT_COUNT] = { transformIntensity, transformRGB, transformR, transformG, transformB }; for(uint32_t i = 0; i < point_count; i++) { ogre_tools::PointCloud::Point& current_point = points[ i ]; funcs[type]( chan.vals[i], current_point, min_intensity, max_intensity, diff_intensity ); } } { RenderAutoLock renderLock( this ); scene_node_->setPosition( position ); scene_node_->setOrientation( orientation ); cloud_->clear(); if ( !points.empty() ) { cloud_->addPoints( &points.front(), points.size() ); } } causeRender(); }