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(); }
void PointsMarkerCustom::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message) { ROS_ASSERT(new_message->type == visualization_msgs::Marker::POINTS || new_message->type == visualization_msgs::Marker::CUBE_LIST || new_message->type == visualization_msgs::Marker::SPHERE_LIST); if (!points_) { points_ = new PointCloudCustom(); scene_node_->attachObject(points_); } Ogre::Vector3 pos, scale; Ogre::Quaternion orient; transform(new_message, pos, orient, scale); switch (new_message->type) { case visualization_msgs::Marker::POINTS: points_->setRenderMode(PointCloudCustom::RM_SQUARES); points_->setDimensions(new_message->scale.x, new_message->scale.y, 0.0f); break; case visualization_msgs::Marker::CUBE_LIST: points_->setRenderMode(PointCloudCustom::RM_BOXES); points_->setDimensions(scale.x, scale.y, scale.z); break; case visualization_msgs::Marker::SPHERE_LIST: points_->setRenderMode(PointCloudCustom::RM_SPHERES); points_->setDimensions(scale.x, scale.y, scale.z); break; } setPosition(pos); setOrientation(orient); points_->clear(); if (new_message->points.empty()) { return; } float r = new_message->color.r; float g = new_message->color.g; float b = new_message->color.b; float a = new_message->color.a; bool has_per_point_color = new_message->colors.size() == new_message->points.size(); bool has_nonzero_alpha = false; bool has_per_point_alpha = false; typedef std::vector< PointCloudCustom::Point > V_Point; V_Point points; points.resize(new_message->points.size()); std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin(); std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end(); for (int i = 0; it != end; ++it, ++i) { const geometry_msgs::Point& p = *it; PointCloudCustom::Point& point = points[i]; Ogre::Vector3 v(p.x, p.y, p.z); point.position.x = v.x; point.position.y = v.y; point.position.z = v.z; if (has_per_point_color) { const std_msgs::ColorRGBA& color = new_message->colors[i]; r = color.r; g = color.g; b = color.b; a = color.a; has_nonzero_alpha = has_nonzero_alpha || a != 0.0; has_per_point_alpha = has_per_point_alpha || a != 1.0; } point.setColor(r, g, b, a); } if (has_per_point_color) { if (!has_nonzero_alpha ) { owner_->setMarkerStatus(getID(), StatusProperty::Warn, "All points have a zero alpha value."); } points_->setAlpha(1.0, has_per_point_alpha); } else { points_->setAlpha(a); } points_->addPoints(&points.front(), points.size()); handler_.reset( new MarkerSelectionHandlerCustom( this, MarkerID( new_message->ns, new_message->id ), context_ )); points_->setPickColor( SelectionManager::handleToColor( handler_->getHandle() )); }