00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "camera_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/render_panel.h"
00033 #include "rviz/properties/property.h"
00034 #include "rviz/properties/property_manager.h"
00035 #include "rviz/window_manager_interface.h"
00036 #include "rviz/frame_manager.h"
00037 #include "rviz/validate_floats.h"
00038 #include "rviz/panel_dock_widget.h"
00039 #include "rviz/display_wrapper.h"
00040 #include "rviz/uniform_string_stream.h"
00041
00042 #include <tf/transform_listener.h>
00043
00044 #include <boost/bind.hpp>
00045
00046 #include <rviz/ogre_helpers/axes.h>
00047
00048 #include <OGRE/OgreSceneNode.h>
00049 #include <OGRE/OgreSceneManager.h>
00050 #include <OGRE/OgreRectangle2D.h>
00051 #include <OGRE/OgreMaterialManager.h>
00052 #include <OGRE/OgreTextureManager.h>
00053 #include <OGRE/OgreViewport.h>
00054 #include <OGRE/OgreRenderWindow.h>
00055 #include <OGRE/OgreManualObject.h>
00056 #include <OGRE/OgreRoot.h>
00057 #include <OGRE/OgreRenderSystem.h>
00058
00059 namespace rviz
00060 {
00061
00062 static const std::string IMAGE_POS_BACKGROUND = "background";
00063 static const std::string IMAGE_POS_OVERLAY = "overlay";
00064 static const std::string IMAGE_POS_BOTH = "background & overlay";
00065
00066 bool validateFloats(const sensor_msgs::CameraInfo& msg)
00067 {
00068 bool valid = true;
00069 valid = valid && validateFloats(msg.D);
00070 valid = valid && validateFloats(msg.K);
00071 valid = valid && validateFloats(msg.R);
00072 valid = valid && validateFloats(msg.P);
00073 return valid;
00074 }
00075
00076 CameraDisplay::CameraDisplay()
00077 : Display()
00078 , zoom_(1)
00079 , transport_("raw")
00080 , image_position_(IMAGE_POS_BOTH)
00081 , caminfo_tf_filter_( 0 )
00082 , new_caminfo_(false)
00083 , texture_(update_nh_)
00084 , render_panel_( 0 )
00085 , force_render_(false)
00086 , panel_container_( 0 )
00087 {
00088 }
00089
00090 CameraDisplay::~CameraDisplay()
00091 {
00092 unsubscribe();
00093 caminfo_tf_filter_->clear();
00094
00095 if( render_panel_ )
00096 {
00097 if( panel_container_ )
00098 {
00099 delete panel_container_;
00100 }
00101 else
00102 {
00103 delete render_panel_;
00104 }
00105 }
00106
00107 delete bg_screen_rect_;
00108 delete fg_screen_rect_;
00109
00110 bg_scene_node_->getParentSceneNode()->removeAndDestroyChild(bg_scene_node_->getName());
00111 fg_scene_node_->getParentSceneNode()->removeAndDestroyChild(fg_scene_node_->getName());
00112
00113 delete caminfo_tf_filter_;
00114 }
00115
00116 void CameraDisplay::onInitialize()
00117 {
00118 caminfo_tf_filter_ = new tf::MessageFilter<sensor_msgs::CameraInfo>(*vis_manager_->getTFClient(), "", 2, update_nh_);
00119
00120 bg_scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00121 fg_scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00122
00123 {
00124 static int count = 0;
00125 UniformStringStream ss;
00126 ss << "CameraDisplayObject" << count++;
00127
00128
00129 bg_screen_rect_ = new Ogre::Rectangle2D(true);
00130 bg_screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00131
00132 ss << "Material";
00133 bg_material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
00134 bg_material_->setDepthWriteEnabled(false);
00135
00136 bg_material_->setReceiveShadows(false);
00137 bg_material_->setDepthCheckEnabled(false);
00138
00139 bg_material_->getTechnique(0)->setLightingEnabled(false);
00140 Ogre::TextureUnitState* tu = bg_material_->getTechnique(0)->getPass(0)->createTextureUnitState();
00141 tu->setTextureName(texture_.getTexture()->getName());
00142 tu->setTextureFiltering( Ogre::TFO_NONE );
00143 tu->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, 0.0 );
00144
00145 bg_material_->setCullingMode(Ogre::CULL_NONE);
00146 bg_material_->setSceneBlending( Ogre::SBT_REPLACE );
00147
00148 Ogre::AxisAlignedBox aabInf;
00149 aabInf.setInfinite();
00150
00151 bg_screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_BACKGROUND);
00152 bg_screen_rect_->setBoundingBox(aabInf);
00153 bg_screen_rect_->setMaterial(bg_material_->getName());
00154
00155 bg_scene_node_->attachObject(bg_screen_rect_);
00156 bg_scene_node_->setVisible(false);
00157
00158
00159 fg_screen_rect_ = new Ogre::Rectangle2D(true);
00160 fg_screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00161
00162 fg_material_ = bg_material_->clone( ss.str()+"fg" );
00163 fg_screen_rect_->setBoundingBox(aabInf);
00164 fg_screen_rect_->setMaterial(fg_material_->getName());
00165
00166 fg_material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00167 fg_screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1);
00168
00169 fg_scene_node_->attachObject(fg_screen_rect_);
00170 fg_scene_node_->setVisible(false);
00171 }
00172
00173 setAlpha( 0.5f );
00174
00175 render_panel_ = new RenderPanel();
00176 render_panel_->getRenderWindow()->addListener( this );
00177 render_panel_->getRenderWindow()->setAutoUpdated(false);
00178 render_panel_->getRenderWindow()->setActive( false );
00179 render_panel_->resize( 640, 480 );
00180 render_panel_->initialize(vis_manager_->getSceneManager(), vis_manager_);
00181
00182 WindowManagerInterface* wm = vis_manager_->getWindowManager();
00183 if( wm )
00184 {
00185 panel_container_ = wm->addPane(name_, render_panel_);
00186 }
00187 render_panel_->setAutoRender(false);
00188 render_panel_->setOverlaysEnabled(false);
00189 render_panel_->getCamera()->setNearClipDistance( 0.01f );
00190
00191 caminfo_tf_filter_->connectInput(caminfo_sub_);
00192 caminfo_tf_filter_->registerCallback(boost::bind(&CameraDisplay::caminfoCallback, this, _1));
00193 vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(caminfo_tf_filter_, this);
00194
00195 if( panel_container_ )
00196 {
00197
00198 connect( panel_container_, SIGNAL( visibilityChanged( bool ) ), this, SLOT( setWrapperEnabled( bool )));
00199 }
00200 }
00201
00202 void CameraDisplay::preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
00203 {
00204 bg_scene_node_->setVisible( image_position_ == IMAGE_POS_BACKGROUND || image_position_ == IMAGE_POS_BOTH );
00205 fg_scene_node_->setVisible( image_position_ == IMAGE_POS_OVERLAY || image_position_ == IMAGE_POS_BOTH );
00206 }
00207
00208 void CameraDisplay::postRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
00209 {
00210 bg_scene_node_->setVisible(false);
00211 fg_scene_node_->setVisible(false);
00212 }
00213
00214 void CameraDisplay::setWrapperEnabled( bool enabled )
00215 {
00216
00217
00218 DisplayWrapper* wrapper = vis_manager_->getDisplayWrapper( this );
00219 if( wrapper != NULL )
00220 {
00221 wrapper->setEnabled( enabled );
00222 }
00223 }
00224
00225 void CameraDisplay::onEnable()
00226 {
00227 subscribe();
00228 if( render_panel_->parentWidget() == 0 )
00229 {
00230 render_panel_->show();
00231 }
00232 else
00233 {
00234 panel_container_->show();
00235 }
00236
00237 render_panel_->getRenderWindow()->setActive(true);
00238 }
00239
00240 void CameraDisplay::onDisable()
00241 {
00242 render_panel_->getRenderWindow()->setActive(false);
00243
00244 if( render_panel_->parentWidget() == 0 )
00245 {
00246 if( render_panel_->isVisible() )
00247 {
00248 render_panel_->hide();
00249 }
00250 }
00251 else
00252 {
00253 if( panel_container_->isVisible() )
00254 {
00255 panel_container_->hide();
00256 }
00257 }
00258
00259 unsubscribe();
00260
00261 clear();
00262 }
00263
00264 void CameraDisplay::subscribe()
00265 {
00266 if ( !isEnabled() )
00267 {
00268 return;
00269 }
00270
00271 try
00272 {
00273 texture_.setTopic(topic_);
00274 setStatus( status_levels::Ok, "Topic", "OK" );
00275 }
00276 catch( ros::Exception& e )
00277 {
00278 setStatus( status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what() );
00279 }
00280
00281
00282 std::string caminfo_topic = "camera_info";
00283 size_t pos = topic_.rfind('/');
00284 if (pos != std::string::npos)
00285 {
00286 std::string ns = topic_;
00287 ns.erase(pos);
00288
00289 caminfo_topic = ns + "/" + caminfo_topic;
00290 }
00291
00292 try
00293 {
00294 caminfo_sub_.subscribe(update_nh_, caminfo_topic, 1);
00295 setStatus( status_levels::Ok, "Camera Info Topic", "OK" );
00296 }
00297 catch( ros::Exception& e )
00298 {
00299 setStatus( status_levels::Error, "Camera Info Topic", std::string("Error subscribing: ") + e.what() );
00300 }
00301 }
00302
00303 void CameraDisplay::unsubscribe()
00304 {
00305 texture_.setTopic("");
00306 caminfo_sub_.unsubscribe();
00307 }
00308
00309 void CameraDisplay::setAlpha( float alpha )
00310 {
00311 alpha_ = alpha;
00312
00313 Ogre::Pass* pass = fg_material_->getTechnique(0)->getPass(0);
00314 if (pass->getNumTextureUnitStates() > 0)
00315 {
00316 Ogre::TextureUnitState* tex_unit = pass->getTextureUnitState(0);
00317 tex_unit->setAlphaOperation( Ogre::LBX_MODULATE, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha_ );
00318 }
00319 else
00320 {
00321 fg_material_->setAmbient(Ogre::ColourValue(0.0f, 1.0f, 1.0f, alpha_));
00322 fg_material_->setDiffuse(Ogre::ColourValue(0.0f, 1.0f, 1.0f, alpha_));
00323 }
00324
00325 propertyChanged(alpha_property_);
00326 force_render_ = true;
00327 causeRender();
00328 }
00329
00330 void CameraDisplay::setZoom( float zoom )
00331 {
00332 if (fabs(zoom) < .00001 || fabs(zoom) > 100000)
00333 {
00334 return;
00335 }
00336 zoom_ = zoom;
00337
00338 propertyChanged(zoom_property_);
00339
00340 force_render_ = true;
00341 causeRender();
00342 }
00343
00344 void CameraDisplay::setQueueSize( int size )
00345 {
00346 if( size != (int) caminfo_tf_filter_->getQueueSize() )
00347 {
00348 texture_.setQueueSize( (uint32_t) size );
00349 caminfo_tf_filter_->setQueueSize( (uint32_t) size );
00350 propertyChanged( queue_size_property_ );
00351 }
00352 }
00353
00354 int CameraDisplay::getQueueSize()
00355 {
00356 return (int) caminfo_tf_filter_->getQueueSize();
00357 }
00358
00359 void CameraDisplay::setTopic( const std::string& topic )
00360 {
00361 unsubscribe();
00362
00363 topic_ = topic;
00364 clear();
00365
00366 subscribe();
00367
00368 propertyChanged(topic_property_);
00369 }
00370
00371 void CameraDisplay::setTransport(const std::string& transport)
00372 {
00373 transport_ = transport;
00374
00375 texture_.setTransportType(transport);
00376
00377 propertyChanged(transport_property_);
00378 }
00379
00380 void CameraDisplay::setImagePosition(const std::string& image_position)
00381 {
00382 image_position_ = image_position;
00383
00384 propertyChanged(image_position_property_);
00385
00386 force_render_ = true;
00387 causeRender();
00388 }
00389
00390 void CameraDisplay::clear()
00391 {
00392 texture_.clear();
00393 force_render_ = true;
00394 causeRender();
00395
00396 new_caminfo_ = false;
00397 current_caminfo_.reset();
00398
00399 setStatus(status_levels::Warn, "CameraInfo", "No CameraInfo received on [" + caminfo_sub_.getTopic() + "]. Topic may not exist.");
00400 setStatus(status_levels::Warn, "Image", "No Image received");
00401
00402 render_panel_->getCamera()->setPosition(Ogre::Vector3(999999, 999999, 999999));
00403 }
00404
00405 void CameraDisplay::updateStatus()
00406 {
00407 if (texture_.getImageCount() == 0)
00408 {
00409 setStatus(status_levels::Warn, "Image", "No image received");
00410 }
00411 else
00412 {
00413 std::stringstream ss;
00414 ss << texture_.getImageCount() << " images received";
00415 setStatus(status_levels::Ok, "Image", ss.str());
00416 }
00417 }
00418
00419 void CameraDisplay::update(float wall_dt, float ros_dt)
00420 {
00421 updateStatus();
00422
00423 try
00424 {
00425 if (texture_.update() || force_render_)
00426 {
00427 float old_alpha = alpha_;
00428 if (texture_.getImageCount() == 0)
00429 {
00430 alpha_ = 1.0f;
00431 }
00432
00433 updateCamera();
00434 render_panel_->getRenderWindow()->update();
00435 alpha_ = old_alpha;
00436
00437 force_render_ = false;
00438 }
00439 }
00440 catch (UnsupportedImageEncoding& e)
00441 {
00442 setStatus(status_levels::Error, "Image", e.what());
00443 }
00444 }
00445
00446 void CameraDisplay::updateCamera()
00447 {
00448 sensor_msgs::CameraInfo::ConstPtr info;
00449 sensor_msgs::Image::ConstPtr image;
00450 {
00451 boost::mutex::scoped_lock lock(caminfo_mutex_);
00452
00453 info = current_caminfo_;
00454 image = texture_.getImage();
00455 }
00456
00457 if (!info || !image)
00458 {
00459 return;
00460 }
00461
00462 if (!validateFloats(*info))
00463 {
00464 setStatus(status_levels::Error, "CameraInfo", "Contains invalid floating point values (nans or infs)");
00465 return;
00466 }
00467
00468 Ogre::Vector3 position;
00469 Ogre::Quaternion orientation;
00470
00471 vis_manager_->getFrameManager()->getTransform(image->header.frame_id, ros::Time(0), position, orientation);
00472
00473
00474 orientation = orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X);
00475
00476 float img_width = info->width;
00477 float img_height = info->height;
00478
00479
00480 if (img_width == 0)
00481 {
00482 ROS_DEBUG("Malformed CameraInfo on camera [%s], width = 0", getName().c_str());
00483
00484 img_width = texture_.getWidth();
00485 }
00486
00487 if (img_height == 0)
00488 {
00489 ROS_DEBUG("Malformed CameraInfo on camera [%s], height = 0", getName().c_str());
00490
00491 img_height = texture_.getHeight();
00492 }
00493
00494 if (img_height == 0.0 || img_width == 0.0)
00495 {
00496 setStatus(status_levels::Error, "CameraInfo", "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)");
00497 return;
00498 }
00499
00500 double fx = info->P[0];
00501 double fy = info->P[5];
00502
00503 float win_width = render_panel_->width();
00504 float win_height = render_panel_->height();
00505 float zoom_x = zoom_;
00506 float zoom_y = zoom_;
00507
00508
00509 if ( win_width != 0 && win_height != 0 )
00510 {
00511 float img_aspect = (img_width/fx) / (img_height/fy);
00512 float win_aspect = win_width / win_height;
00513
00514 if ( img_aspect > win_aspect )
00515 {
00516 zoom_y = zoom_y / img_aspect * win_aspect;
00517 }
00518 else
00519 {
00520 zoom_x = zoom_x / win_aspect * img_aspect;
00521 }
00522 }
00523
00524
00525 double tx = -1 * (info->P[3] / fx);
00526 Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
00527 position = position + (right * tx);
00528
00529 double ty = -1 * (info->P[7] / fy);
00530 Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y;
00531 position = position + (down * ty);
00532
00533 if (!validateFloats(position))
00534 {
00535 setStatus(status_levels::Error, "CameraInfo", "CameraInfo/P resulted in an invalid position calculation (nans or infs)");
00536 return;
00537 }
00538
00539 render_panel_->getCamera()->setPosition(position);
00540 render_panel_->getCamera()->setOrientation(orientation);
00541
00542
00543 double cx = info->P[2];
00544 double cy = info->P[6];
00545
00546 double far_plane = 100;
00547 double near_plane = 0.01;
00548
00549 Ogre::Matrix4 proj_matrix;
00550 proj_matrix = Ogre::Matrix4::ZERO;
00551
00552 proj_matrix[0][0]= 2.0 * fx/img_width * zoom_x;
00553 proj_matrix[1][1]= 2.0 * fy/img_height * zoom_y;
00554
00555 proj_matrix[0][2]= 2.0 * (0.5 - cx/img_width) * zoom_x;
00556 proj_matrix[1][2]= 2.0 * (cy/img_height - 0.5) * zoom_y;
00557
00558 proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane);
00559 proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane);
00560
00561 proj_matrix[3][2]= -1;
00562
00563 render_panel_->getCamera()->setCustomProjectionMatrix( true, proj_matrix );
00564
00565 setStatus(status_levels::Ok, "CameraInfo", "OK");
00566
00567 #if 0
00568 static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01);
00569 debug_axes->setPosition(position);
00570 debug_axes->setOrientation(orientation);
00571 #endif
00572
00573
00574 bg_screen_rect_->setCorners(-1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y);
00575 fg_screen_rect_->setCorners(-1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y);
00576
00577 Ogre::AxisAlignedBox aabInf;
00578 aabInf.setInfinite();
00579 bg_screen_rect_->setBoundingBox(aabInf);
00580 fg_screen_rect_->setBoundingBox(aabInf);
00581 }
00582
00583 void CameraDisplay::caminfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
00584 {
00585 boost::mutex::scoped_lock lock(caminfo_mutex_);
00586 current_caminfo_ = msg;
00587 new_caminfo_ = true;
00588 }
00589
00590 void CameraDisplay::onTransportEnumOptions(V_string& choices)
00591 {
00592 texture_.getAvailableTransportTypes(choices);
00593 }
00594
00595 void CameraDisplay::onImagePositionEnumOptions(V_string& choices)
00596 {
00597 choices.clear();
00598 choices.push_back(IMAGE_POS_BACKGROUND);
00599 choices.push_back(IMAGE_POS_OVERLAY);
00600 choices.push_back(IMAGE_POS_BOTH);
00601 }
00602
00603 void CameraDisplay::createProperties()
00604 {
00605 topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Image Topic", property_prefix_, boost::bind( &CameraDisplay::getTopic, this ),
00606 boost::bind( &CameraDisplay::setTopic, this, _1 ), parent_category_, this );
00607 setPropertyHelpText(topic_property_, "sensor_msgs::Image topic to subscribe to. The topic must be a well-formed <strong>camera</strong> topic, and in order to work properly must have a matching <strong>camera_info<strong> topic.");
00608 ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00609 topic_prop->setMessageType(ros::message_traits::datatype<sensor_msgs::Image>());
00610
00611 transport_property_ = property_manager_->createProperty<EditEnumProperty>("Transport Hint", property_prefix_, boost::bind(&CameraDisplay::getTransport, this),
00612 boost::bind(&CameraDisplay::setTransport, this, _1), parent_category_, this);
00613 EditEnumPropertyPtr transport_prop = transport_property_.lock();
00614 transport_prop->setOptionCallback(boost::bind(&CameraDisplay::onTransportEnumOptions, this, _1));
00615
00616 image_position_property_ = property_manager_->createProperty<EditEnumProperty>("Image Rendering", property_prefix_, boost::bind(&CameraDisplay::getImagePosition, this),
00617 boost::bind(&CameraDisplay::setImagePosition, this, _1), parent_category_, this);
00618 setPropertyHelpText(image_position_property_, "Render the image behind all other geometry or overlay it on top.");
00619 EditEnumPropertyPtr ip_prop = image_position_property_.lock();
00620 ip_prop->setOptionCallback(boost::bind(&CameraDisplay::onImagePositionEnumOptions, this, _1));
00621
00622 alpha_property_ = property_manager_->createProperty<FloatProperty>( "Overlay Alpha", property_prefix_, boost::bind( &CameraDisplay::getAlpha, this ),
00623 boost::bind( &CameraDisplay::setAlpha, this, _1 ), parent_category_, this );
00624 setPropertyHelpText(alpha_property_, "The amount of transparency to apply to the camera image when rendered as overlay.");
00625
00626 zoom_property_ = property_manager_->createProperty<FloatProperty>("Zoom Factor", property_prefix_, boost::bind(&CameraDisplay::getZoom, this),
00627 boost::bind( &CameraDisplay::setZoom, this, _1), parent_category_, this);
00628 setPropertyHelpText(image_position_property_, "Set a zoom factor below 1 to see a larger part of the world, a factor above 1 to magnify the image.");
00629
00630 queue_size_property_ = property_manager_->createProperty<IntProperty>( "Queue Size", property_prefix_,
00631 boost::bind( &CameraDisplay::getQueueSize, this ),
00632 boost::bind( &CameraDisplay::setQueueSize, this, _1 ),
00633 parent_category_, this );
00634 setPropertyHelpText( queue_size_property_, "Advanced: set the size of the incoming message queue. Increasing this is useful if your incoming TF data is delayed significantly from your camera data, but it can greatly increase memory usage if the messages are big." );
00635 }
00636
00637 void CameraDisplay::fixedFrameChanged()
00638 {
00639 caminfo_tf_filter_->setTargetFrame(fixed_frame_);
00640 texture_.setFrame(fixed_frame_, vis_manager_->getTFClient());
00641 }
00642
00643 void CameraDisplay::reset()
00644 {
00645 Display::reset();
00646
00647 clear();
00648 }
00649
00650 }