30 #include <boost/bind.hpp> 32 #include <OgreManualObject.h> 33 #include <OgreMaterialManager.h> 34 #include <OgreRectangle2D.h> 35 #include <OgreRenderSystem.h> 36 #include <OgreRenderWindow.h> 37 #include <OgreSceneManager.h> 38 #include <OgreSceneNode.h> 39 #include <OgreTextureManager.h> 40 #include <OgreViewport.h> 41 #include <OgreTechnique.h> 42 #include <OgreCamera.h> 85 , caminfo_tf_filter_( 0 )
86 , new_caminfo_( false )
87 , force_render_( false )
91 "Render the image behind all other geometry or overlay it on top, or both.",
98 "The amount of transparency to apply to the camera image when rendered as overlay.",
104 "Set a zoom factor below 1 to see a larger part of the world, above 1 to magnify the image.",
147 static int count = 0;
149 ss <<
"CameraDisplayObject" << count++;
156 bg_material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
162 bg_material_->getTechnique(0)->setLightingEnabled(
false);
163 Ogre::TextureUnitState* tu =
bg_material_->getTechnique(0)->getPass(0)->createTextureUnitState();
165 tu->setTextureFiltering( Ogre::TFO_NONE );
166 tu->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, 0.0 );
171 Ogre::AxisAlignedBox aabInf;
172 aabInf.setInfinite();
179 bg_scene_node_->setVisible(
false);
189 fg_material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
220 "Changes the visibility of other Displays in the camera view.");
292 Ogre::Pass* pass =
fg_material_->getTechnique( 0 )->getPass( 0 );
293 if( pass->getNumTextureUnitStates() > 0 )
295 Ogre::TextureUnitState* tex_unit = pass->getTextureUnitState( 0 );
296 tex_unit->setAlphaOperation( Ogre::LBX_MODULATE, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha );
300 fg_material_->setAmbient( Ogre::ColourValue( 0.0
f, 1.0
f, 1.0
f, alpha ));
301 fg_material_->setDiffuse( Ogre::ColourValue( 0.0
f, 1.0
f, 1.0
f, alpha ));
330 "No CameraInfo received on [" + QString::fromStdString(
caminfo_sub_.
getTopic() ) +
"]. Topic may not exist.");
356 sensor_msgs::CameraInfo::ConstPtr info;
357 sensor_msgs::Image::ConstPtr image;
365 if( !info || !image )
379 rviz_time != image->header.stamp )
381 std::ostringstream
s;
382 s <<
"Time-syncing active and no image at timestamp " << rviz_time.
toSec() <<
".";
387 Ogre::Vector3 position;
388 Ogre::Quaternion orientation;
394 orientation = orientation * Ogre::Quaternion( Ogre::Degree( 180 ), Ogre::Vector3::UNIT_X );
396 float img_width = info->width;
397 float img_height = info->height;
402 ROS_DEBUG(
"Malformed CameraInfo on camera [%s], width = 0", qPrintable(
getName() ));
408 ROS_DEBUG(
"Malformed CameraInfo on camera [%s], height = 0", qPrintable(
getName() ));
412 if( img_height == 0.0 || img_width == 0.0 )
415 "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)" );
419 double fx = info->P[0];
420 double fy = info->P[5];
425 float zoom_y = zoom_x;
428 if( win_width != 0 && win_height != 0 )
430 float img_aspect = (img_width/fx) / (img_height/fy);
431 float win_aspect = win_width / win_height;
433 if ( img_aspect > win_aspect )
435 zoom_y = zoom_y / img_aspect * win_aspect;
439 zoom_x = zoom_x / win_aspect * img_aspect;
444 double tx = -1 * (info->P[3] / fx);
445 Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
446 position = position + (right * tx);
448 double ty = -1 * (info->P[7] / fy);
449 Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y;
450 position = position + (down * ty);
462 double cx = info->P[2];
463 double cy = info->P[6];
465 double far_plane = 100;
466 double near_plane = 0.01;
468 Ogre::Matrix4 proj_matrix;
469 proj_matrix = Ogre::Matrix4::ZERO;
471 proj_matrix[0][0]= 2.0 * fx/img_width * zoom_x;
472 proj_matrix[1][1]= 2.0 * fy/img_height * zoom_y;
474 proj_matrix[0][2]= 2.0 * (0.5 - cx/img_width) * zoom_x;
475 proj_matrix[1][2]= 2.0 * (cy/img_height - 0.5) * zoom_y;
477 proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane);
478 proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane);
480 proj_matrix[3][2]= -1;
493 double x_corner_start, y_corner_start, x_corner_end, y_corner_end;
495 if ( info->roi.height != 0 || info->roi.width != 0 )
498 x_corner_start = (2.0 * info->roi.x_offset / info->width - 1.0) * zoom_x;
499 y_corner_start = (-2.0 * info->roi.y_offset / info->height + 1.0) * zoom_y;
500 x_corner_end = x_corner_start + (2.0 * info->roi.width / info->width) * zoom_x;
501 y_corner_end = y_corner_start - (2.0 * info->roi.height / info->height) * zoom_y;
505 x_corner_start = -1.0f*zoom_x;
506 y_corner_start = 1.0f*zoom_y;
507 x_corner_end = 1.0f*zoom_x;
508 y_corner_end = -1.0f*zoom_y;
511 bg_screen_rect_->setCorners( x_corner_start, y_corner_start, x_corner_end, y_corner_end);
512 fg_screen_rect_->setCorners( x_corner_start, y_corner_start, x_corner_end, y_corner_end);
514 Ogre::AxisAlignedBox aabInf;
515 aabInf.setInfinite();
std::string getTopic() const
FloatProperty * alpha_property_
const Ogre::TexturePtr & getTexture()
void subscribe()
ROS topic management.
sensor_msgs::CameraInfo::ConstPtr current_caminfo_
Ogre::SceneNode * bg_scene_node_
void initialize(Ogre::SceneManager *scene_manager, DisplayContext *manager)
static const QString OVERLAY
static const QString BOTH
virtual tf::TransformListener * getTFClient() const =0
Convenience function: returns getFrameManager()->getTFClient().
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
virtual void processMessage(const sensor_msgs::Image::ConstPtr &msg)
Implement this to process the contents of a message.
void setAutoRender(bool auto_render)
virtual void onInitialize()
Override this function to do subclass-specific initialization.
virtual int getInt() const
Return the internal property value as an integer.
virtual void fixedFrameChanged()
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
RenderPanel * render_panel_
virtual float getFloat() const
tf::MessageFilter< sensor_msgs::CameraInfo > * caminfo_tf_filter_
Ogre::SceneNode * fg_scene_node_
virtual void unsubscribe()
virtual void updateQueueSize()
Property specialized to enforce floating point max/min.
ros::Time getTime()
Get current time, depending on the sync mode.
boost::mutex caminfo_mutex_
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
virtual void fixedFrameChanged()
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
virtual DisplayGroup * getRootDisplayGroup() const =0
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
bool isEnabled() const
Return true if this Display is enabled, false if not.
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
virtual void setQueueSize(uint32_t new_queue_size)
virtual void setIcon(const QIcon &icon)
Set the icon to be displayed next to the property.
IntProperty * queue_size_property_
EnumProperty * image_position_property_
virtual void onEnable()
Derived classes override this to do the actual work of enabling themselves.
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual void addOption(const QString &option, int value=0)
virtual void subscribe()
ROS topic management.
Display subclass for subscribing and displaying to image messages.
void enableTFFilter(std::string &targetFrame)
Enabling TF filtering by defining a target frame.
void addMessage(const sensor_msgs::Image::ConstPtr &image)
virtual BitAllocator * visibilityBits()=0
virtual void onDisable()
Derived classes override this to do the actual work of disabling themselves.
void setAssociatedWidget(QWidget *widget)
Associate the given widget with this Display.
Ogre::Rectangle2D * fg_screen_rect_
virtual void updateQueueSize()
Update queue size of tf filter.
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
DisplayGroupVisibilityProperty * visibility_property_
virtual void reset()
Reset display.
std::string getCameraInfoTopic(const std::string &base_topic)
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
Ogre::SceneManager * scene_manager_
A convenience variable equal to context_->getSceneManager().
virtual void onInitialize()
Override this function to do subclass-specific initialization.
FloatProperty * zoom_property_
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
virtual void reset()
Reset display.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
Ogre::Viewport * getViewport() const
RosTopicProperty * topic_property_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
Ogre::Camera * getCamera() const
virtual void addChild(Property *child, int index=-1)
Add a child property.
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
virtual void postRenderTargetUpdate(const Ogre::RenderTargetEvent &evt)
Ogre::MaterialPtr fg_material_
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
static const QString BACKGROUND
void setTargetFrame(const std::string &target_frame)
Ogre::Rectangle2D * bg_screen_rect_
bool initialized() const
Returns true if the display has been initialized.
const sensor_msgs::Image::ConstPtr & getImage()
std::string getTopicStd() const
virtual void preRenderTargetUpdate(const Ogre::RenderTargetEvent &evt)
message_filters::Subscriber< sensor_msgs::CameraInfo > caminfo_sub_
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
Ogre::MaterialPtr bg_material_
void freeBits(uint32_t bits)
Free the given bits.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
QPixmap loadPixmap(QString url, bool fill_cache)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
virtual QString getName() const
Return the name of this Property as a QString.
void caminfoCallback(const sensor_msgs::CameraInfo::ConstPtr &msg)
void setOverlaysEnabled(bool overlays_enabled)
Connection registerCallback(const C &callback)
uint32_t allocBit()
Return a uint32 with a single bit "on" (previously unused), or a 0 if all bits are already allocated...