Skip to content

Commit

Permalink
publish a point topic whenever you click on the image
Browse files Browse the repository at this point in the history
  • Loading branch information
@jolting
jolting committed Sep 10, 2015
1 parent b49b20c commit fc6922c
Show file tree
Hide file tree
Showing 2 changed files with 183 additions and 104 deletions.
203 changes: 128 additions & 75 deletions src/rviz/default_plugin/image_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <boost/bind.hpp>

#include <QMouseEvent>
#include <OgreManualObject.h>
#include <OgreMaterialManager.h>
#include <OgreRectangle2D.h>
Expand All @@ -53,38 +52,50 @@

#include "image_display.h"

namespace rviz
{
namespace rviz {

ImageDisplay::ImageDisplay()
: ImageDisplayBase()
, texture_()
{
normalize_property_ = new BoolProperty( "Normalize Range", true,
"If set to true, will try to estimate the range of possible values from the received images.",
this, SLOT( updateNormalizeOptions() ));
: ImageDisplayBase(), texture_(), img_scale_(0), img_xoffset_(0),
img_yoffset_(0), img_width_(0), img_height_(0) {
normalize_property_ = new BoolProperty(
"Normalize Range", true, "If set to true, will try to estimate the range "
"of possible values from the received images.",
this, SLOT(updateNormalizeOptions()));

min_property_ = new FloatProperty("Min Value", 0.0,
"Value which will be displayed as black.",
this, SLOT(updateNormalizeOptions()));

max_property_ = new FloatProperty("Max Value", 1.0,
"Value which will be displayed as white.",
this, SLOT(updateNormalizeOptions()));

min_property_ = new FloatProperty( "Min Value", 0.0, "Value which will be displayed as black.", this, SLOT( updateNormalizeOptions() ));
median_buffer_size_property_ = new IntProperty(
"Median window", 5,
"Window size for median filter used for computin min/max.", this,
SLOT(updateNormalizeOptions()));

max_property_ = new FloatProperty( "Max Value", 1.0, "Value which will be displayed as white.", this, SLOT( updateNormalizeOptions() ));
topic_property_ = new StringProperty("Topic", "/image_clicked_point",
"The topic on which to publish points.",
this, SLOT(updateTopic()));

median_buffer_size_property_ = new IntProperty( "Median window", 5, "Window size for median filter used for computin min/max.",
this, SLOT( updateNormalizeOptions() ) );
updateTopic();

got_float_image_ = false;
}

void ImageDisplay::onInitialize()
{
void ImageDisplay::onInitialize() {
ImageDisplayBase::onInitialize();
{
static uint32_t count = 0;
std::stringstream ss;
ss << "ImageDisplay" << count++;
img_scene_manager_ = Ogre::Root::getSingleton().createSceneManager(Ogre::ST_GENERIC, ss.str());
img_scene_manager_ = Ogre::Root::getSingleton().createSceneManager(
Ogre::ST_GENERIC, ss.str());
}

img_scene_node_ = img_scene_manager_->getRootSceneNode()->createChildSceneNode();
img_scene_node_ =
img_scene_manager_->getRootSceneNode()->createChildSceneNode();

{
static int count = 0;
Expand All @@ -96,16 +107,18 @@ void ImageDisplay::onInitialize()
screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);

ss << "Material";
material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
material_->setSceneBlending( Ogre::SBT_REPLACE );
material_ = Ogre::MaterialManager::getSingleton().create(
ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
material_->setSceneBlending(Ogre::SBT_REPLACE);
material_->setDepthWriteEnabled(false);
material_->setReceiveShadows(false);
material_->setDepthCheckEnabled(false);

material_->getTechnique(0)->setLightingEnabled(false);
Ogre::TextureUnitState* tu = material_->getTechnique(0)->getPass(0)->createTextureUnitState();
Ogre::TextureUnitState *tu =
material_->getTechnique(0)->getPass(0)->createTextureUnitState();
tu->setTextureName(texture_.getTexture()->getName());
tu->setTextureFiltering( Ogre::TFO_NONE );
tu->setTextureFiltering(Ogre::TFO_NONE);

material_->setCullingMode(Ogre::CULL_NONE);
Ogre::AxisAlignedBox aabInf;
Expand All @@ -115,137 +128,177 @@ void ImageDisplay::onInitialize()
img_scene_node_->attachObject(screen_rect_);
}

render_panel_ = new RenderPanel();
MouseEventHandler mevent =
boost::bind(&ImageDisplay::mouseEventHandler, this, _1);
render_panel_ = new InteractiveRenderPanel();
connect(render_panel_, SIGNAL(mouseEventHandler(QMouseEvent *)), this,
SLOT(mouseEventHandler(QMouseEvent *)));
render_panel_->getRenderWindow()->setAutoUpdated(false);
render_panel_->getRenderWindow()->setActive( false );
render_panel_->getRenderWindow()->setActive(false);

render_panel_->resize( 640, 480 );
render_panel_->resize(640, 480);
render_panel_->initialize(img_scene_manager_, context_);

setAssociatedWidget( render_panel_ );
setAssociatedWidget(render_panel_);

render_panel_->setAutoRender(false);
render_panel_->setOverlaysEnabled(false);
render_panel_->getCamera()->setNearClipDistance( 0.01f );
render_panel_->getCamera()->setNearClipDistance(0.01f);

updateNormalizeOptions();
}

ImageDisplay::~ImageDisplay()
{
if ( initialized() )
{
ImageDisplay::~ImageDisplay() {
if (initialized()) {
delete render_panel_;
delete screen_rect_;
img_scene_node_->getParentSceneNode()->removeAndDestroyChild( img_scene_node_->getName() );
img_scene_node_->getParentSceneNode()->removeAndDestroyChild(
img_scene_node_->getName());
}
}

void ImageDisplay::onEnable()
{
void ImageDisplay::onEnable() {
ImageDisplayBase::subscribe();
render_panel_->getRenderWindow()->setActive(true);
}

void ImageDisplay::onDisable()
{
void ImageDisplay::onDisable() {
render_panel_->getRenderWindow()->setActive(false);
ImageDisplayBase::unsubscribe();
clear();
}

void ImageDisplay::updateNormalizeOptions()
{
if (got_float_image_)
{
void ImageDisplay::updateNormalizeOptions() {
if (got_float_image_) {
bool normalize = normalize_property_->getBool();

normalize_property_->setHidden(false);
min_property_->setHidden(normalize);
max_property_->setHidden(normalize);
median_buffer_size_property_->setHidden(!normalize);

texture_.setNormalizeFloatImage( normalize, min_property_->getFloat(), max_property_->getFloat());
texture_.setMedianFrames( median_buffer_size_property_->getInt() );
}
else
{
texture_.setNormalizeFloatImage(normalize, min_property_->getFloat(),
max_property_->getFloat());
texture_.setMedianFrames(median_buffer_size_property_->getInt());
} else {
normalize_property_->setHidden(true);
min_property_->setHidden(true);
max_property_->setHidden(true);
median_buffer_size_property_->setHidden(true);
}
}

void ImageDisplay::clear()
{
void ImageDisplay::clear() {
texture_.clear();

if( render_panel_->getCamera() )
{
render_panel_->getCamera()->setPosition(Ogre::Vector3(999999, 999999, 999999));
if (render_panel_->getCamera()) {
render_panel_->getCamera()->setPosition(
Ogre::Vector3(999999, 999999, 999999));
}
}

void ImageDisplay::update( float wall_dt, float ros_dt )
{
try
{
void ImageDisplay::update(float wall_dt, float ros_dt) {
try {
texture_.update();

//make sure the aspect ratio of the image is preserved
// make sure the aspect ratio of the image is preserved
float win_width = render_panel_->width();
float win_height = render_panel_->height();

float img_width = texture_.getWidth();
float img_height = texture_.getHeight();

if ( img_width != 0 && img_height != 0 && win_width !=0 && win_height != 0 )
{
if (img_width != 0 && img_height != 0 && win_width != 0 &&
win_height != 0) {
float img_aspect = img_width / img_height;
float win_aspect = win_width / win_height;

if ( img_aspect > win_aspect )
{
screen_rect_->setCorners(-1.0f, 1.0f * win_aspect/img_aspect, 1.0f, -1.0f * win_aspect/img_aspect, false);
}
else
{
screen_rect_->setCorners(-1.0f * img_aspect/win_aspect, 1.0f, 1.0f * img_aspect/win_aspect, -1.0f, false);
if (img_aspect > win_aspect) {
img_scale_ = img_width / win_width;
img_xoffset_ = 0;
img_yoffset_ =
2 * win_height / img_aspect - img_height / img_scale_ / 2;

screen_rect_->setCorners(-1.0f, 1.0f * win_aspect / img_aspect, 1.0f,
-1.0f * win_aspect / img_aspect, false);
} else {
img_scale_ = img_height / win_height;
img_xoffset_ = 2 * win_width / img_aspect - img_width / img_scale_ / 2;
img_yoffset_ = 0;

screen_rect_->setCorners(-1.0f * img_aspect / win_aspect, 1.0f,
1.0f * img_aspect / win_aspect, -1.0f, false);
}
img_width_ = img_width;
img_height_ = img_height;
}

render_panel_->getRenderWindow()->update();
}
catch( UnsupportedImageEncoding& e )
{
} catch (UnsupportedImageEncoding &e) {
setStatus(StatusProperty::Error, "Image", e.what());
}
}

void ImageDisplay::reset()
{
void ImageDisplay::reset() {
ImageDisplayBase::reset();
clear();
}

/* This is called by incomingMessage(). */
void ImageDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg)
{
bool got_float_image = msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1 ||
void ImageDisplay::processMessage(const sensor_msgs::Image::ConstPtr &msg) {
bool got_float_image =
msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1 ||
msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
msg->encoding == sensor_msgs::image_encodings::TYPE_16SC1 ||
msg->encoding == sensor_msgs::image_encodings::MONO16;

if ( got_float_image != got_float_image_ )
{
if (got_float_image != got_float_image_) {
got_float_image_ = got_float_image;
updateNormalizeOptions();
}
texture_.addMessage(msg);
}

void ImageDisplay::mouseEventHandler(QMouseEvent *mevent) {
if (mevent->button() == Qt::NoButton)
return;
geometry_msgs::PointStamped ps;

ps.point.x = (mevent->pos().x() - img_xoffset_) * img_scale_;
ps.point.y = (mevent->pos().y() - img_yoffset_) * img_scale_;
ps.point.z = 0;
if (ps.point.x < 0)
ps.point.x = 0;
else if (ps.point.y < 0)
ps.point.y = 0;

if (ps.point.x > img_width_)
ps.point.x = img_width_;
else if (ps.point.y > img_height_)
ps.point.y = img_height_;

ps.header.frame_id = context_->getFixedFrame().toStdString();
ps.header.stamp = ros::Time::now();
pub_.publish(ps);
}
void ImageDisplay::updateTopic() {
pub_ = nh_.advertise<geometry_msgs::PointStamped>(
topic_property_->getStdString(), 1);
}
void InteractiveRenderPanel::mouseMoveEvent(QMouseEvent *event) {
Q_EMIT mouseEventHandler(event);
}
void InteractiveRenderPanel::mousePressEvent(QMouseEvent *event) {
Q_EMIT mouseEventHandler(event);
}
void InteractiveRenderPanel::mouseReleaseEvent(QMouseEvent *event) {
Q_EMIT mouseEventHandler(event);
}
void InteractiveRenderPanel::mouseDoubleClickEvent(QMouseEvent *event) {
Q_EMIT mouseEventHandler(event);
}

} // namespace rviz

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rviz::ImageDisplay, rviz::Display )
PLUGINLIB_EXPORT_CLASS(rviz::ImageDisplay, rviz::Display)
Loading

0 comments on commit fc6922c

Please sign in to comment.