bool validateFloats(const nav_msgs::OccupancyGrid& msg) { bool valid = true; valid = valid && validateFloats( msg.info.resolution ); valid = valid && validateFloats( msg.info.origin ); return valid; }
bool validateFloats(const nav_msgs::Odometry& msg) { bool valid = true; valid = valid && validateFloats( msg.pose.pose ); valid = valid && validateFloats( msg.twist.twist ); return valid; }
bool validateFloats(const sensor_msgs::CameraInfo& msg) { bool valid = true; valid = valid && validateFloats(msg.D); valid = valid && validateFloats(msg.K); valid = valid && validateFloats(msg.R); valid = valid && validateFloats(msg.P); return valid; }
bool validateFloats(const visualization_msgs::Marker& msg) { bool valid = true; valid = valid && validateFloats(msg.pose); valid = valid && validateFloats(msg.scale); valid = valid && validateFloats(msg.color); valid = valid && validateFloats(msg.points); return valid; }
void InteractiveMarkerDisplay::updatePoses( const std::string& server_id, const std::vector<visualization_msgs::InteractiveMarkerPose>& marker_poses ) { M_StringToIMPtr& im_map = getImMap( server_id ); for ( size_t i=0; i<marker_poses.size(); i++ ) { const visualization_msgs::InteractiveMarkerPose& marker_pose = marker_poses[i]; if ( !validateFloats( marker_pose.pose ) ) { setStatusStd( StatusProperty::Error, marker_pose.name, "Pose message contains invalid floats!" ); return; } std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker_pose.name ); if ( int_marker_entry != im_map.end() ) { int_marker_entry->second->processMessage( marker_pose ); } else { setStatusStd( StatusProperty::Error, marker_pose.name, "Pose received for non-existing marker '" + marker_pose.name ); unsubscribe(); return; } } }
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 PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr& msg ) { if( !validateFloats( *msg )) { setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" ); return; } manual_object_->clear(); Ogre::Vector3 position; Ogre::Quaternion orientation; if( !context_->getFrameManager()->getTransform( msg->header, position, orientation )) { ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); } scene_node_->setPosition( position ); scene_node_->setOrientation( orientation ); manual_object_->clear(); Ogre::ColourValue color = color_property_->getOgreColor(); float length = length_property_->getFloat(); size_t num_poses = msg->poses.size(); manual_object_->estimateVertexCount( num_poses * 6 ); manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST ); for( size_t i=0; i < num_poses; ++i ) { Ogre::Vector3 pos( msg->poses[i].position.x, msg->poses[i].position.y, msg->poses[i].position.z ); Ogre::Quaternion orient( msg->poses[i].orientation.w, msg->poses[i].orientation.x, msg->poses[i].orientation.y, msg->poses[i].orientation.z ); // orient here is not normalized, so the scale of the quaternion // will affect the scale of the arrow. Ogre::Vector3 vertices[6]; vertices[0] = pos; // back of arrow vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 ); // tip of arrow vertices[2] = vertices[ 1 ]; vertices[3] = pos + orient * Ogre::Vector3( 0.75*length, 0.2*length, 0 ); vertices[4] = vertices[ 1 ]; vertices[5] = pos + orient * Ogre::Vector3( 0.75*length, -0.2*length, 0 ); for( int i = 0; i < 6; ++i ) { manual_object_->position( vertices[i] ); manual_object_->colour( color ); } } manual_object_->end(); context_->queueRender(); }
void PointCloud2Display::processMessage( const sensor_msgs::PointCloud2ConstPtr& cloud ) { // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase // will get their points put off in lala land, but it means they still do get processed/rendered // which can be a big performance hit sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2); int32_t xi = findChannelIndex(cloud, "x"); int32_t yi = findChannelIndex(cloud, "y"); int32_t zi = findChannelIndex(cloud, "z"); if (xi == -1 || yi == -1 || zi == -1) { return; } const uint32_t xoff = cloud->fields[xi].offset; const uint32_t yoff = cloud->fields[yi].offset; const uint32_t zoff = cloud->fields[zi].offset; const uint32_t point_step = cloud->point_step; const size_t point_count = cloud->width * cloud->height; filtered->data.resize(cloud->data.size()); if (point_count == 0) { return; } uint32_t output_count = 0; const uint8_t* ptr = &cloud->data.front(); for (size_t i = 0; i < point_count; ++i) { float x = *reinterpret_cast<const float*>(ptr + xoff); float y = *reinterpret_cast<const float*>(ptr + yoff); float z = *reinterpret_cast<const float*>(ptr + zoff); if (validateFloats(Ogre::Vector3(x, y, z))) { memcpy(&filtered->data.front() + (output_count * point_step), ptr, point_step); ++output_count; } ptr += point_step; } filtered->header = cloud->header; filtered->fields = cloud->fields; filtered->data.resize(output_count * point_step); filtered->height = 1; filtered->width = output_count; filtered->is_bigendian = cloud->is_bigendian; filtered->point_step = point_step; filtered->row_step = output_count; point_cloud_common_->addMessage( filtered ); }
void PolygonDisplay::processMessage(const geometry_msgs::PolygonStamped::ConstPtr& msg) { if( !validateFloats( *msg )) { setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" ); return; } Ogre::Vector3 position; Ogre::Quaternion orientation; if( !context_->getFrameManager()->getTransform( msg->header, position, orientation )) { ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); } scene_node_->setPosition( position ); scene_node_->setOrientation( orientation ); manual_object_->clear(); Ogre::ColourValue color = qtToOgre( color_property_->getColor() ); color.a = alpha_property_->getFloat(); // TODO: this does not actually support alpha as-is. The // "BaseWhiteNoLighting" material ends up ignoring the alpha // component of the color values we set at each point. Need to make // a material and do the whole setSceneBlending() rigamarole. uint32_t num_points = msg->polygon.points.size(); if( num_points > 0 ) { manual_object_->estimateVertexCount( num_points ); manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP ); for( uint32_t i=0; i < num_points + 1; ++i ) { const geometry_msgs::Point32& msg_point = msg->polygon.points[ i % num_points ]; manual_object_->position( msg_point.x, msg_point.y, msg_point.z ); manual_object_->colour( color ); } manual_object_->end(); } }
bool validateFloats(const visualization_msgs::InteractiveMarker& msg) { bool valid = true; valid = valid && validateFloats(msg.pose); valid = valid && validateFloats(msg.scale); for ( unsigned c=0; c<msg.controls.size(); c++) { valid = valid && validateFloats( msg.controls[c].orientation ); for ( unsigned m=0; m<msg.controls[c].markers.size(); m++ ) { valid = valid && validateFloats(msg.controls[c].markers[m].pose); valid = valid && validateFloats(msg.controls[c].markers[m].scale); valid = valid && validateFloats(msg.controls[c].markers[m].color); valid = valid && validateFloats(msg.controls[c].markers[m].points); } } return valid; }
void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& message ) { ++messages_received_; if( !validateFloats( *message )) { setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" ); return; } setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" ); if( last_used_message_ ) { Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z); Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z); Ogre::Quaternion last_orientation(last_used_message_->pose.pose.orientation.w, last_used_message_->pose.pose.orientation.x, last_used_message_->pose.pose.orientation.y, last_used_message_->pose.pose.orientation.z); Ogre::Quaternion current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z); if( (last_position - current_position).length() < position_tolerance_property_->getFloat() && (last_orientation - current_orientation).normalise() < angle_tolerance_property_->getFloat() ) { return; } } Arrow* arrow = new Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f ); transformArrow( message, arrow ); QColor color = color_property_->getColor(); arrow->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f ); float length = length_property_->getFloat(); Ogre::Vector3 scale( length, length, length ); arrow->setScale( scale ); arrows_.push_back( arrow ); last_used_message_ = message; context_->queueRender(); }
void PolygonArrayDisplay::processMessage(const jsk_recognition_msgs::PolygonArray::ConstPtr& msg) { if (!validateFloats(*msg)) { setStatus(rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)"); return; } // create nodes and manual objects updateSceneNodes(msg); allocateMaterials(msg->polygons.size()); updateLines(msg->polygons.size()); if (only_border_) { // use line_ for (size_t i = 0; i < manual_objects_.size(); i++) { manual_objects_[i]->setVisible(false); } for (size_t i = 0; i < msg->polygons.size(); i++) { geometry_msgs::PolygonStamped polygon = msg->polygons[i]; if (polygon.polygon.points.size() >= 3) { processLine(i, polygon); } } } else { for (size_t i = 0; i < msg->polygons.size(); i++) { processPolygonMaterial(i); } for (size_t i = 0; i < msg->polygons.size(); i++) { geometry_msgs::PolygonStamped polygon = msg->polygons[i]; processPolygon(i, polygon); } } if (show_normal_) { for (size_t i = 0; i < msg->polygons.size(); i++) { geometry_msgs::PolygonStamped polygon = msg->polygons[i]; processNormal(i, polygon); } } }
void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg ) { Ogre::ManualObject* manual_object = manual_objects_[ messages_received_ % buffer_length_property_->getInt() ]; manual_object->clear(); if( !validateFloats( *msg )) { setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" ); return; } Ogre::Vector3 position; Ogre::Quaternion orientation; if( !context_->getFrameManager()->getTransform( msg->header, position, orientation )) { ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); } Ogre::Matrix4 transform( orientation ); transform.setTrans( position ); // scene_node_->setPosition( position ); // scene_node_->setOrientation( orientation ); Ogre::ColourValue color = color_property_->getOgreColor(); color.a = alpha_property_->getFloat(); uint32_t num_points = msg->poses.size(); manual_object->estimateVertexCount( num_points ); manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP ); for( uint32_t i=0; i < num_points; ++i) { const geometry_msgs::Point& pos = msg->poses[ i ].pose.position; Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z ); manual_object->position( xpos.x, xpos.y, xpos.z ); manual_object->colour( color ); } manual_object->end(); }
void MarkerDisplay::processMessage( const visualization_msgs::Marker::ConstPtr& message ) { if (!validateFloats(*message)) { setMarkerStatus(MarkerID(message->ns, message->id), StatusProperty::Error, "Contains invalid floating point values (nans or infs)"); return; } switch ( message->action ) { case visualization_msgs::Marker::ADD: processAdd( message ); break; case visualization_msgs::Marker::DELETE: processDelete( message ); break; default: ROS_ERROR( "Unknown marker action: %d\n", message->action ); } }
void InteractiveMarkerDisplay::updateMarkers( const std::string& server_id, const std::vector<visualization_msgs::InteractiveMarker>& markers ) { M_StringToIMPtr& im_map = getImMap( server_id ); for ( size_t i=0; i<markers.size(); i++ ) { const visualization_msgs::InteractiveMarker& marker = markers[i]; if ( !validateFloats( marker ) ) { setStatusStd( StatusProperty::Error, marker.name, "Marker contains invalid floats!" ); //setStatusStd( StatusProperty::Error, "General", "Marker " + marker.name + " contains invalid floats!" ); continue; } ROS_DEBUG("Processing interactive marker '%s'. %d", marker.name.c_str(), (int)marker.controls.size() ); std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker.name ); if ( int_marker_entry == im_map.end() ) { int_marker_entry = im_map.insert( std::make_pair(marker.name, IMPtr ( new InteractiveMarker(this, context_, topic_ns_, client_id_) ) ) ).first; } if ( int_marker_entry->second->processMessage( marker ) ) { int_marker_entry->second->setShowAxes( show_axes_property_->getBool() ); int_marker_entry->second->setShowDescription( show_descriptions_property_->getBool() ); } else { unsubscribe(); return; } } }
bool CameraPub::updateCamera() { sensor_msgs::CameraInfo::ConstPtr info; { boost::mutex::scoped_lock lock(caminfo_mutex_); info = current_caminfo_; } if (!info) { return false; } if (!validateFloats(*info)) { setStatus(StatusProperty::Error, "Camera Info", "Contains invalid floating point values (nans or infs)"); return false; } // if we're in 'exact' time mode, only show image if the time is exactly right ros::Time rviz_time = context_->getFrameManager()->getTime(); if (context_->getFrameManager()->getSyncMode() == FrameManager::SyncExact && rviz_time != info->header.stamp) { std::ostringstream s; s << "Time-syncing active and no info at timestamp " << rviz_time.toSec() << "."; setStatus(StatusProperty::Warn, "Time", s.str().c_str()); return false; } // TODO(lucasw) this will make the img vs. texture size code below unnecessary if ((info->width != render_texture_->getWidth()) || (info->height != render_texture_->getHeight())) { rtt_texture_ = Ogre::TextureManager::getSingleton().createManual( "RttTex", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, info->width, info->height, 0, Ogre::PF_R8G8B8, Ogre::TU_RENDERTARGET); render_texture_ = rtt_texture_->getBuffer()->getRenderTarget(); render_texture_->addViewport(camera_); render_texture_->getViewport(0)->setClearEveryFrame(true); render_texture_->getViewport(0)->setBackgroundColour(Ogre::ColourValue::Black); render_texture_->getViewport(0)->setVisibilityMask(vis_bit_); render_texture_->getViewport(0)->setOverlaysEnabled(false); render_texture_->setAutoUpdated(false); render_texture_->setActive(false); render_texture_->addListener(this); } Ogre::Vector3 position; Ogre::Quaternion orientation; context_->getFrameManager()->getTransform(info->header.frame_id, info->header.stamp, position, orientation); // printf( "CameraPub:updateCamera(): pos = %.2f, %.2f, %.2f.\n", position.x, position.y, position.z ); // convert vision (Z-forward) frame to ogre frame (Z-out) orientation = orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X); float img_width = info->width; float img_height = info->height; // If the image width is 0 due to a malformed caminfo, try to grab the width from the image. if (img_width == 0) { ROS_DEBUG("Malformed CameraInfo on camera [%s], width = 0", qPrintable(getName())); img_width = 640; } if (img_height == 0) { ROS_DEBUG("Malformed CameraInfo on camera [%s], height = 0", qPrintable(getName())); img_height = 480; } if (img_height == 0.0 || img_width == 0.0) { setStatus(StatusProperty::Error, "Camera Info", "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)"); return false; } double fx = info->P[0]; double fy = info->P[5]; float win_width = render_texture_->getWidth(); float win_height = render_texture_->getHeight(); float zoom_x = 1.0; float zoom_y = zoom_x; // Preserve aspect ratio if (win_width != 0 && win_height != 0) { float img_aspect = (img_width / fx) / (img_height / fy); float win_aspect = win_width / win_height; if (img_aspect > win_aspect) { zoom_y = zoom_y / img_aspect * win_aspect; } else { zoom_x = zoom_x / win_aspect * img_aspect; } } // Add the camera's translation relative to the left camera (from P[3]); double tx = -1 * (info->P[3] / fx); Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X; position = position + (right * tx); double ty = -1 * (info->P[7] / fy); Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y; position = position + (down * ty); if (!validateFloats(position)) { setStatus(StatusProperty::Error, "Camera Info", "CameraInfo/P resulted in an invalid position calculation (nans or infs)"); return false; } camera_->setPosition(position); camera_->setOrientation(orientation); // calculate the projection matrix double cx = info->P[2]; double cy = info->P[6]; double far_plane = 100; double near_plane = 0.01; Ogre::Matrix4 proj_matrix; proj_matrix = Ogre::Matrix4::ZERO; proj_matrix[0][0] = 2.0 * fx / img_width * zoom_x; proj_matrix[1][1] = 2.0 * fy / img_height * zoom_y; proj_matrix[0][2] = 2.0 * (0.5 - cx / img_width) * zoom_x; proj_matrix[1][2] = 2.0 * (cy / img_height - 0.5) * zoom_y; proj_matrix[2][2] = -(far_plane + near_plane) / (far_plane - near_plane); proj_matrix[2][3] = -2.0 * far_plane * near_plane / (far_plane - near_plane); proj_matrix[3][2] = -1; camera_->setCustomProjectionMatrix(true, proj_matrix); setStatus(StatusProperty::Ok, "Camera Info", "OK"); #if 0 static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01); debug_axes->setPosition(position); debug_axes->setOrientation(orientation); #endif setStatus(StatusProperty::Ok, "Time", "ok"); return true; }
void CameraDisplay::updateCamera() { sensor_msgs::CameraInfo::ConstPtr info; sensor_msgs::Image::ConstPtr image; { boost::mutex::scoped_lock lock( caminfo_mutex_ ); info = current_caminfo_; image = texture_.getImage(); } if( !info || !image ) { return; } if( !validateFloats( *info )) { setStatus( StatusProperty::Error, "Camera Info", "Contains invalid floating point values (nans or infs)" ); return; } Ogre::Vector3 position; Ogre::Quaternion orientation; context_->getFrameManager()->getTransform( image->header.frame_id, ros::Time(0), position, orientation ); //printf( "CameraDisplay:updateCamera(): pos = %.2f, %.2f, %.2f.\n", position.x, position.y, position.z ); // convert vision (Z-forward) frame to ogre frame (Z-out) orientation = orientation * Ogre::Quaternion( Ogre::Degree( 180 ), Ogre::Vector3::UNIT_X ); float img_width = info->width; float img_height = info->height; // If the image width is 0 due to a malformed caminfo, try to grab the width from the image. if( img_width == 0 ) { ROS_DEBUG( "Malformed CameraInfo on camera [%s], width = 0", qPrintable( getName() )); img_width = texture_.getWidth(); } if (img_height == 0) { ROS_DEBUG( "Malformed CameraInfo on camera [%s], height = 0", qPrintable( getName() )); img_height = texture_.getHeight(); } if( img_height == 0.0 || img_width == 0.0 ) { setStatus( StatusProperty::Error, "Camera Info", "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)" ); return; } double fx = info->P[0]; double fy = info->P[5]; float win_width = render_panel_->width(); float win_height = render_panel_->height(); float zoom_x = zoom_property_->getFloat(); float zoom_y = zoom_x; // Preserve aspect ratio if( win_width != 0 && win_height != 0 ) { float img_aspect = (img_width/fx) / (img_height/fy); float win_aspect = win_width / win_height; if ( img_aspect > win_aspect ) { zoom_y = zoom_y / img_aspect * win_aspect; } else { zoom_x = zoom_x / win_aspect * img_aspect; } } // Add the camera's translation relative to the left camera (from P[3]); double tx = -1 * (info->P[3] / fx); Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X; position = position + (right * tx); double ty = -1 * (info->P[7] / fy); Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y; position = position + (down * ty); if( !validateFloats( position )) { setStatus( StatusProperty::Error, "Camera Info", "CameraInfo/P resulted in an invalid position calculation (nans or infs)" ); return; } render_panel_->getCamera()->setPosition( position ); render_panel_->getCamera()->setOrientation( orientation ); // calculate the projection matrix double cx = info->P[2]; double cy = info->P[6]; double far_plane = 100; double near_plane = 0.01; Ogre::Matrix4 proj_matrix; proj_matrix = Ogre::Matrix4::ZERO; proj_matrix[0][0]= 2.0 * fx/img_width * zoom_x; proj_matrix[1][1]= 2.0 * fy/img_height * zoom_y; proj_matrix[0][2]= 2.0 * (0.5 - cx/img_width) * zoom_x; proj_matrix[1][2]= 2.0 * (cy/img_height - 0.5) * zoom_y; proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane); proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane); proj_matrix[3][2]= -1; render_panel_->getCamera()->setCustomProjectionMatrix( true, proj_matrix ); setStatus( StatusProperty::Ok, "Camera Info", "OK" ); #if 0 static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01); debug_axes->setPosition(position); debug_axes->setOrientation(orientation); #endif //adjust the image rectangles to fit the zoom & aspect ratio bg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y ); fg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y ); Ogre::AxisAlignedBox aabInf; aabInf.setInfinite(); bg_screen_rect_->setBoundingBox( aabInf ); fg_screen_rect_->setBoundingBox( aabInf ); }
bool validateFloats( const geometry_msgs::PoseArray& msg ) { return validateFloats( msg.poses ); }
void MapDisplay::showMap() { if (current_map_.data.empty()) { return; } if( !validateFloats( current_map_ )) { setStatus( StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or infs)" ); return; } if( current_map_.info.width * current_map_.info.height == 0 ) { std::stringstream ss; ss << "Map is zero-sized (" << current_map_.info.width << "x" << current_map_.info.height << ")"; setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() )); return; } setStatus( StatusProperty::Ok, "Message", "Map received" ); ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n", current_map_.info.width, current_map_.info.height, current_map_.info.resolution ); float resolution = current_map_.info.resolution; int width = current_map_.info.width; int height = current_map_.info.height; Ogre::Vector3 position( current_map_.info.origin.position.x, current_map_.info.origin.position.y, current_map_.info.origin.position.z ); Ogre::Quaternion orientation( current_map_.info.origin.orientation.w, current_map_.info.origin.orientation.x, current_map_.info.origin.orientation.y, current_map_.info.origin.orientation.z ); frame_ = current_map_.header.frame_id; if (frame_.empty()) { frame_ = "/map"; } unsigned int pixels_size = width * height; unsigned char* pixels = new unsigned char[pixels_size]; memset(pixels, 255, pixels_size); bool map_status_set = false; unsigned int num_pixels_to_copy = pixels_size; if( pixels_size != current_map_.data.size() ) { std::stringstream ss; ss << "Data size doesn't match width*height: width = " << width << ", height = " << height << ", data size = " << current_map_.data.size(); setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() )); map_status_set = true; // Keep going, but don't read past the end of the data. if( current_map_.data.size() < pixels_size ) { num_pixels_to_copy = current_map_.data.size(); } } memcpy( pixels, ¤t_map_.data[0], num_pixels_to_copy ); Ogre::DataStreamPtr pixel_stream; pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size )); if( !texture_.isNull() ) { Ogre::TextureManager::getSingleton().remove( texture_->getName() ); texture_.setNull(); } static int tex_count = 0; std::stringstream ss; ss << "MapTexture" << tex_count++; try { texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D, 0); if( !map_status_set ) { setStatus( StatusProperty::Ok, "Map", "Map OK" ); } } catch(Ogre::RenderingAPIException&) { Ogre::Image image; pixel_stream->seek(0); float fwidth = width; float fheight = height; if( width > height ) { float aspect = fheight / fwidth; fwidth = 2048; fheight = fwidth * aspect; } else { float aspect = fwidth / fheight; fheight = 2048; fwidth = fheight * aspect; } { std::stringstream ss; ss << "Map is larger than your graphics card supports. Downsampled from [" << width << "x" << height << "] to [" << fwidth << "x" << fheight << "]"; setStatus(StatusProperty::Ok, "Map", QString::fromStdString( ss.str() )); } ROS_WARN("Failed to create full-size map texture, likely because your graphics card does not support textures of size > 2048. Downsampling to [%d x %d]...", (int)fwidth, (int)fheight); //ROS_INFO("Stream size [%d], width [%f], height [%f], w * h [%f]", pixel_stream->size(), width, height, width * height); image.loadRawData(pixel_stream, width, height, Ogre::PF_L8); image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST); ss << "Downsampled"; texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image); } delete [] pixels; Ogre::Pass* pass = material_->getTechnique(0)->getPass(0); Ogre::TextureUnitState* tex_unit = NULL; if (pass->getNumTextureUnitStates() > 0) { tex_unit = pass->getTextureUnitState(0); } else { tex_unit = pass->createTextureUnitState(); } tex_unit->setTextureName(texture_->getName()); tex_unit->setTextureFiltering( Ogre::TFO_NONE ); updatePalette(); resolution_property_->setValue( resolution ); width_property_->setValue( width ); height_property_->setValue( height ); position_property_->setVector( position ); orientation_property_->setQuaternion( orientation ); transformMap(); manual_object_->setVisible( true ); scene_node_->setScale( resolution * width, resolution * height, 1.0 ); context_->queueRender(); }
void DepthCloudDisplay::convert(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::ImageConstPtr& color_msg, const sensor_msgs::CameraInfo::ConstPtr camInfo_msg, sensor_msgs::PointCloud2Ptr& cloud_msg) { int width = depth_msg->width; int height = depth_msg->height; ////////////////////////////// // initialize cloud message ////////////////////////////// cloud_msg->header = depth_msg->header; if (color_msg) { cloud_msg->fields.resize(4); cloud_msg->fields[0].name = "x"; cloud_msg->fields[1].name = "y"; cloud_msg->fields[2].name = "z"; cloud_msg->fields[3].name = "rgb"; } else { cloud_msg->fields.resize(3); cloud_msg->fields[0].name = "x"; cloud_msg->fields[1].name = "y"; cloud_msg->fields[2].name = "z"; } int offset = 0; // All offsets are *4, as all field data types are float32 for (size_t d = 0; d < cloud_msg->fields.size(); ++d, offset += 4) { cloud_msg->fields[d].offset = offset; cloud_msg->fields[d].datatype = sensor_msgs::PointField::FLOAT32; cloud_msg->fields[d].count = 1; } cloud_msg->point_step = offset; cloud_msg->data.resize(height * width * offset); cloud_msg->is_bigendian = false; cloud_msg->is_dense = false; ////////////////////////////// // Update camera model ////////////////////////////// image_geometry::PinholeCameraModel cameraModel; if (camInfo_msg) { cameraModel.fromCameraInfo(camInfo_msg); } else { setStatus(StatusProperty::Error, "Message", QString("Waiting for CameraInfo message..")); return; } // Use correct principal point from calibration float center_x = cameraModel.cx(); float center_y = cameraModel.cy(); // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) float constant_x = 1.0f / cameraModel.fx(); float constant_y = 1.0f / cameraModel.fy(); ////////////////////////////// // Color conversion ////////////////////////////// unsigned char* colorImgPtr = 0; if (color_msg) { if (depth_msg->header.frame_id != color_msg->header.frame_id) { std::stringstream errorMsg; errorMsg << "Depth image frame id [" << depth_msg->header.frame_id.c_str() << "] doesn't match RGB image frame id [" << color_msg->header.frame_id.c_str() << "]"; setStatus(StatusProperty::Error, "Message", QString(errorMsg.str().c_str()) ); return; } if (depth_msg->width != color_msg->width || depth_msg->height != color_msg->height) { std::stringstream errorMsg; errorMsg << "Depth resolution (" << (int)depth_msg->width << "x" << (int)depth_msg->height << ") " "does not match RGB resolution (" << (int)color_msg->width << "x" << (int)color_msg->height << ")"; setStatus(StatusProperty::Error, "Message", QString(errorMsg.str().c_str()) ); return; } // OpenCV-ros bridge cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(color_msg, "rgba8"); } catch (cv_bridge::Exception& e) { setStatus(StatusProperty::Error, "Message", QString("OpenCV-ROS bridge: ") + e.what()); return; } catch (cv::Exception& e) { setStatus(StatusProperty::Error, "Message", QString("OpenCV: ") + e.what()); return; } colorImgPtr = (unsigned char*)&cv_ptr->image.data[0]; } //////////////////////////////////////////////// // depth map to point cloud conversion //////////////////////////////////////////////// float* cloudDataPtr = reinterpret_cast<float*>(&cloud_msg->data[0]); size_t pointCount_ = 0; Ogre::Vector3 newPoint; Ogre::Vector3 transPoint; const T* img_ptr = (T*)&depth_msg->data[0]; for (int v = 0; v < height; ++v) { for (int u = 0; u < width; ++u) { uint32_t color_rgb; if (colorImgPtr) { uint8_t color_r, color_g, color_b; color_r = *colorImgPtr; ++colorImgPtr; color_g = *colorImgPtr; ++colorImgPtr; color_b = *colorImgPtr; ++colorImgPtr; colorImgPtr++; // alpha padding color_rgb = ((uint32_t)color_r << 16 | (uint32_t)color_g << 8 | (uint32_t)color_b); } float depth = DepthTraits<T>::toMeters( *img_ptr++ ); // Missing points denoted by NaNs if (!DepthTraits<T>::valid(depth)) { continue; } // Fill in XYZ newPoint.x = (u - center_x) * depth * constant_x; newPoint.y = (v - center_y) * depth * constant_y; newPoint.z = depth; if (validateFloats(newPoint)) { *cloudDataPtr = newPoint.x; ++cloudDataPtr; *cloudDataPtr = newPoint.y; ++cloudDataPtr; *cloudDataPtr = newPoint.z; ++cloudDataPtr; } ++pointCount_; if (colorImgPtr) { *cloudDataPtr = *reinterpret_cast<float*>(&color_rgb); ++cloudDataPtr; } } } //////////////////////////////////////////////// // finalize pointcloud2 message //////////////////////////////////////////////// cloud_msg->width = pointCount_; cloud_msg->height = 1; cloud_msg->data.resize(cloud_msg->height * cloud_msg->width * cloud_msg->point_step); cloud_msg->row_step = cloud_msg->point_step * cloud_msg->width; }
bool validateFloats( const geometry_msgs::PolygonStamped& msg ) { return validateFloats(msg.polygon.points); }
void Selection3DDisplayCustom::preRenderTargetUpdate( const Ogre::RenderTargetEvent& evt ) { if(initialized_) { if(constant_size_property_->getBool()) { int view_id = 0;//((rviz::VisualizationManager*)context_)->getActiveViewID(); for(int i = 0; i < render_panel_list_.size(); i++) if(render_panel_list_[i]->getRenderWindow() == (Ogre::RenderWindow*)evt.source) view_id = i; //std::cout << "Update " << view_id << std::endl; // get transform from world to fixed frame Ogre::Quaternion selection_orientation(1,0,0,0); Ogre::Vector3 selection_position = selection_position_; transform(selection_position,selection_orientation,"/world",fixed_frame_.toUtf8().constData()); //std::cout << "selection: " << selection_position.x << "," << selection_position.y << "," << selection_position.z << std::endl; // Ogre::Quaternion selection_orientation_roi(1,0,0,0); // Ogre::Vector3 selection_position_roi = selection_position_roi_; // transform(selection_position_roi,selection_orientation_roi,"/world",fixed_frame_.toUtf8().constData()); //std::cout << selection_position_roi.x << "," << selection_position_roi.y << "," << selection_position_roi.z << std::endl; // get camera position to calculate selection marker //std::cout << "selection " << render_panel_->getCamera() << std::endl; Ogre::Vector3 camera_position = this->render_panel_list_[view_id]->getCamera()->getPosition(); //std::cout << this->context_->getSceneManager()->getCameras().size() << std::endl; //Ogre::SceneManager::CameraList camera_list = this->context_->getSceneManager()->getCameras(); //Ogre::SceneManager::CameraIterator camera_list_iterator = this->context_->getSceneManager()->getCameraIterator(); //while(camera_list_iterator.hasMoreElements()) //{ // Ogre::Camera* temp_cam = camera_list_iterator.getNext(); // Ogre::Vector3 temp_pos = temp_cam->getDerivedPosition(); // std::cout << "it " << temp_pos.x << "," << temp_pos.y << "," << temp_pos.z << std::endl; //} //Ogre::Vector3 camera_position2 = this->context_->getSceneManager()->getCamera(); // std::cout << "Camera: " << camera_position.x << "," << camera_position.y << "," << camera_position.z << std::endl; if(this->render_panel_list_[view_id]->getCamera()->getProjectionType() == Ogre::PT_ORTHOGRAPHIC) // if it's ortho, we need to calculate z again { Ogre::Matrix4 m = this->render_panel_list_[view_id]->getCamera()->getProjectionMatrix(); float near = (1+m[2][3])/m[2][2]; float far = -(1-m[2][3])/m[2][2]; float bottom = (1-m[1][3])/m[1][1]; float top = -(1+m[1][3])/m[1][1]; float left = -(1+m[0][3])/m[0][0]; float right = (1-m[0][3])/m[0][0]; //std::cout << "ortho:\n\t" << left << "\n\t" << right << "\n\t" << bottom << "\n\t" << top << "\n\t" << near << "\n\t" << far << std::endl; if(this->render_panel_list_[view_id]->getCamera()->getPosition().z == 500) camera_position.z = fabs(bottom)+fabs(top); else if(this->render_panel_list_[view_id]->getCamera()->getPosition().y == -500) camera_position.y = fabs(bottom)+fabs(top); else if(this->render_panel_list_[view_id]->getCamera()->getPosition().x == 500) camera_position.x = fabs(bottom)+fabs(top); } //std::cout << "camera: " << camera_position.x << "," << camera_position.y << "," << camera_position.z << std::endl; // get the current fov float size = marker_scale_property_->getFloat(); // calculate distance from markers if(validateFloats(selection_position)) { float distance_selection = camera_position.distance(selection_position); //std::cout << distance_selection << std::endl; if(distance_selection != std::numeric_limits<float>::infinity() && !(distance_selection != distance_selection)) // check for inf and nan { if(!selection_position.isNaN()) selection_marker_->setPosition(selection_position); float scale_selection = 2.0f * distance_selection * tan(this->render_panel_list_[view_id]->getCamera()->getFOVy().valueRadians() / 2.0f);//distance_selection*tan(radians)*0.002f; //std::cout << scale_selection << std::endl; if(scale_selection > 0.0f) selection_marker_->setScale(scale_selection*size,scale_selection*size,scale_selection*size); // if(validateFloats(selection_position_roi)) // { // float distance_selection_roi = camera_position.distance(selection_position_roi); // //std::cout << distance_selection_roi << std::endl; // <<< inf // if(distance_selection_roi != std::numeric_limits<float>::infinity() && !(distance_selection_roi != distance_selection_roi)) // { // if(!selection_position_roi.isNaN()) roi_marker_final_->setPosition(selection_position_roi); // float scale_selection_roi = 2.0f * distance_selection_roi * tan(this->render_panel_list_[view_id]->getCamera()->getFOVy().valueRadians() / 2.0f);// = distance_selection_roi*tan(radians)*0.002f; // //std::cout << scale_selection_roi << std::endl; // if(scale_selection_roi > 0.0f) roi_marker_final_->setScale(scale_selection_roi*size,scale_selection_roi*size,scale_selection_roi*size); // Ogre::Vector3 box_position = (selection_position+selection_position_roi)/2.0f; // if(!box_position.isNaN()) roi_marker_box_->setPosition(box_position); // if(!selection_orientation.isNaN()) roi_marker_box_->setOrientation(selection_orientation); // } // } } } } else { Ogre::Vector3 selection_position = selection_position_; Ogre::Quaternion selection_orientation(1,0,0,0); transform(selection_position,selection_orientation,"/world",fixed_frame_.toUtf8().constData()); // only set position if values are valid if(validateFloats(selection_position) && marker_scale_property_->getFloat() > 0.0f) { selection_marker_->setPosition(selection_position); selection_marker_->setScale(marker_scale_property_->getFloat(),marker_scale_property_->getFloat(),marker_scale_property_->getFloat()); } } } context_->queueRender(); }
bool validateFloats( const nav_msgs::Path& msg ) { bool valid = true; valid = valid && validateFloats( msg.poses ); return valid; }
void FootstepDisplay::processMessage(const jsk_footstep_msgs::FootstepArray::ConstPtr& msg) { if (!validateFloats(*msg)) { setStatus(rviz::StatusProperty::Error, "Topic", "message contained invalid floating point values (nans or infs)"); return; } latest_footstep_ = msg; Ogre::Quaternion orientation; Ogre::Vector3 position; if(!context_->getFrameManager()->getTransform( msg->header.frame_id, msg->header.stamp, position, orientation)) { ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); return; } // check thhe length of the shapes_ allocateCubes(msg->footsteps.size()); allocateTexts(msg->footsteps.size()); line_->clear(); line_->setLineWidth(0.01); line_->setNumLines(1); line_->setMaxPointsPerLine(1024); for (size_t i = 0; i < msg->footsteps.size(); i++) { ShapePtr shape = shapes_[i]; rviz::MovableText* text = texts_[i]; Ogre::SceneNode* node = text_nodes_[i]; jsk_footstep_msgs::Footstep footstep = msg->footsteps[i]; Ogre::Vector3 step_position; Ogre::Quaternion step_quaternion; if( !context_->getFrameManager()->transform( msg->header, footstep.pose, step_position, step_quaternion )) { ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'", qPrintable( getName() ), msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); return; } shape->setPosition(step_position); shape->setOrientation(step_quaternion); // size of shape Ogre::Vector3 scale; if (footstep.dimensions.x == 0 && footstep.dimensions.y == 0 && footstep.dimensions.z == 0) { scale[0] = depth_; scale[1] = width_; scale[2] = height_; } else { scale[0] = footstep.dimensions.x; scale[1] = footstep.dimensions.y; scale[2] = footstep.dimensions.z; } shape->setScale(scale); // update the size of text if (footstep.leg == jsk_footstep_msgs::Footstep::LEFT) { text->setCaption("L"); } else if (footstep.leg == jsk_footstep_msgs::Footstep::RIGHT) { text->setCaption("R"); } else { text->setCaption("unknown"); } text->setCharacterHeight(estimateTextSize(footstep)); node->setPosition(step_position); node->setOrientation(step_quaternion); text->setVisible(show_name_); // TODO line_->addPoint(step_position); } //updateFootstepSize(); updateAlpha(); context_->queueRender(); }
void FootstepDisplay::processMessage(const jsk_footstep_msgs::FootstepArray::ConstPtr& msg) { if (!validateFloats(*msg)) { setStatus(rviz::StatusProperty::Error, "Topic", "message contained invalid floating point values (nans or infs)"); return; } latest_footstep_ = msg; Ogre::Quaternion orientation; Ogre::Vector3 position; if( !context_->getFrameManager()->getTransform( msg->header.frame_id, msg->header.stamp, position, orientation )) { ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); return; } // check thhe length of the shapes_ if (msg->footsteps.size() > shapes_.size()) { // need to allocate for (size_t i = shapes_.size(); i < msg->footsteps.size(); i++) { ShapePtr shape; shape.reset(new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(), scene_node_)); shapes_.push_back(shape); } } else if (msg->footsteps.size() < shapes_.size()) { // need to remove shapes_.resize(msg->footsteps.size()); } line_->clear(); line_->setLineWidth(0.01); line_->setNumLines(1); line_->setMaxPointsPerLine(1024); for (size_t i = 0; i < msg->footsteps.size(); i++) { ShapePtr shape = shapes_[i]; jsk_footstep_msgs::Footstep footstep = msg->footsteps[i]; Ogre::Vector3 step_position; Ogre::Quaternion step_quaternion; if( !context_->getFrameManager()->transform( msg->header, footstep.pose, step_position, step_quaternion )) { ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'", qPrintable( getName() ), msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); return; } shape->setPosition(step_position); shape->setOrientation(step_quaternion); line_->addPoint(step_position); } updateFootstepSize(); updateAlpha(); context_->queueRender(); }
void PolygonArrayDisplay::processMessage(const jsk_pcl_ros::PolygonArray::ConstPtr& msg) { if (!validateFloats(*msg)) { setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" ); return; } // create nodes and manual objects if (msg->polygons.size() * 2 > manual_objects_.size()) { for (size_t i = manual_objects_.size(); i < msg->polygons.size() * 2; i++) { Ogre::SceneNode* scene_node = scene_node_->createChildSceneNode(); Ogre::ManualObject* manual_object = scene_manager_->createManualObject(); manual_object->setDynamic( true ); scene_node->attachObject( manual_object ); manual_objects_.push_back(manual_object); scene_nodes_.push_back(scene_node); } } else if (msg->polygons.size() * 2 < manual_objects_.size()) { for (size_t i = msg->polygons.size() * 2; i < manual_objects_.size(); i++) { scene_manager_->destroyManualObject( manual_objects_[i] ); scene_manager_->destroySceneNode( scene_nodes_[i] ); } // resize the array manual_objects_.resize(msg->polygons.size() * 2); scene_nodes_.resize(msg->polygons.size() * 2); } Ogre::ColourValue color = rviz::qtToOgre( color_property_->getColor() ); color.a = alpha_property_->getFloat(); material_->getTechnique(0)->setAmbient( color * 0.5 ); material_->getTechnique(0)->setDiffuse( color ); if ( color.a < 0.9998 ) { material_->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA ); material_->getTechnique(0)->setDepthWriteEnabled( false ); } else { material_->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE ); material_->getTechnique(0)->setDepthWriteEnabled( true ); } for (size_t i = 0; i < msg->polygons.size(); i++) { geometry_msgs::PolygonStamped polygon = msg->polygons[i]; Ogre::SceneNode* scene_node = scene_nodes_[i * 2]; Ogre::ManualObject* manual_object = manual_objects_[i * 2]; Ogre::Vector3 position; Ogre::Quaternion orientation; if( !context_->getFrameManager()->getTransform( polygon.header, position, orientation )) { ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", polygon.header.frame_id.c_str(), qPrintable( fixed_frame_ )); } scene_node->setPosition( position ); scene_node->setOrientation( orientation ); manual_object->clear(); uint32_t num_points = polygon.polygon.points.size(); if( num_points > 0 ) { manual_object->estimateVertexCount( num_points ); manual_object->begin(material_name_, Ogre::RenderOperation::OT_TRIANGLE_FAN ); for( uint32_t i = 0; i < num_points + 1; ++i ) { const geometry_msgs::Point32& msg_point = polygon.polygon.points[ i % num_points ]; manual_object->position( msg_point.x, msg_point.y, msg_point.z ); } manual_object->end(); } } // reverse order for (size_t i = 0; i < msg->polygons.size(); i++) { geometry_msgs::PolygonStamped polygon = msg->polygons[i]; Ogre::SceneNode* scene_node = scene_nodes_[i * 2 + 1]; Ogre::ManualObject* manual_object = manual_objects_[i * 2 + 1]; Ogre::Vector3 position; Ogre::Quaternion orientation; if( !context_->getFrameManager()->getTransform( polygon.header, position, orientation )) { ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", polygon.header.frame_id.c_str(), qPrintable( fixed_frame_ )); } scene_node->setPosition( position ); scene_node->setOrientation( orientation ); manual_object->clear(); uint32_t num_points = polygon.polygon.points.size(); if( num_points > 0 ) { manual_object->estimateVertexCount( num_points ); manual_object->begin(material_name_, Ogre::RenderOperation::OT_TRIANGLE_FAN ); for( uint32_t i = num_points; i > 0; --i ) { const geometry_msgs::Point32& msg_point = polygon.polygon.points[ i % num_points ]; manual_object->position( msg_point.x, msg_point.y, msg_point.z ); } manual_object->end(); } } }
void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg ) { // Calculate index of oldest element in cyclic buffer size_t bufferIndex = messages_received_ % buffer_length_property_->getInt(); LineStyle style = (LineStyle) style_property_->getOptionInt(); Ogre::ManualObject* manual_object = NULL; rviz::BillboardLine* billboard_line = NULL; // Delete oldest element switch(style) { case LINES: manual_object = manual_objects_[ bufferIndex ]; manual_object->clear(); break; case BILLBOARDS: billboard_line = billboard_lines_[ bufferIndex ]; billboard_line->clear(); break; } // Check if path contains invalid coordinate values if( !validateFloats( *msg )) { setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" ); return; } // Lookup transform into fixed frame Ogre::Vector3 position; Ogre::Quaternion orientation; if( !context_->getFrameManager()->getTransform( msg->header, position, orientation )) { ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); } Ogre::Matrix4 transform( orientation ); transform.setTrans( position ); // scene_node_->setPosition( position ); // scene_node_->setOrientation( orientation ); Ogre::ColourValue color = color_property_->getOgreColor(); color.a = alpha_property_->getFloat(); uint32_t num_points = msg->poses.size(); float line_width = line_width_property_->getFloat(); switch(style) { case LINES: manual_object->estimateVertexCount( num_points ); manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP ); for( uint32_t i=0; i < num_points; ++i) { const geometry_msgs::Point& pos = msg->poses[ i ].pose.position; Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z ); manual_object->position( xpos.x, xpos.y, xpos.z ); manual_object->colour( color ); } manual_object->end(); break; case BILLBOARDS: billboard_line->setNumLines( 1 ); billboard_line->setMaxPointsPerLine( num_points ); billboard_line->setLineWidth( line_width ); for( uint32_t i=0; i < num_points; ++i) { const geometry_msgs::Point& pos = msg->poses[ i ].pose.position; Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z ); billboard_line->addPoint( xpos, color ); } break; } }