Skip to content

Commit

Permalink
Cleanup
Browse files Browse the repository at this point in the history
- Rename variables and functions for more clarity
- Drop shared_ptr whereever possible
- Drop MouseClick::updateTopic()
  • Loading branch information
rhaschke committed Apr 25, 2024
1 parent 22fb342 commit 1a48217
Show file tree
Hide file tree
Showing 4 changed files with 34 additions and 77 deletions.
15 changes: 4 additions & 11 deletions src/rviz/default_plugin/image_display.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -131,8 +131,7 @@ void ImageDisplay::onInitialize()

updateNormalizeOptions();

mouse_click_.reset(new MouseClick(render_panel_, update_nh_));
mouse_click_->onInitialize();
mouse_click_ = new MouseClick(render_panel_, update_nh_);
}

ImageDisplay::~ImageDisplay()
Expand All @@ -148,7 +147,7 @@ ImageDisplay::~ImageDisplay()
void ImageDisplay::onEnable()
{
ImageDisplayBase::subscribe();
mouse_click_->publish();
mouse_click_->enable();

render_panel_->getRenderWindow()->setActive(true);
}
Expand All @@ -157,7 +156,7 @@ void ImageDisplay::onDisable()
{
render_panel_->getRenderWindow()->setActive(false);
ImageDisplayBase::unsubscribe();
mouse_click_->unpublish();
mouse_click_->disable();

reset();
}
Expand Down Expand Up @@ -250,16 +249,10 @@ void ImageDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg)
texture_.addMessage(msg);
}

void ImageDisplay::setTopic(const QString& topic, const QString& datatype)
{
ImageDisplayBase::setTopic(topic, datatype);
mouse_click_->setTopic(topic);
}

void ImageDisplay::updateTopic()
{
ImageDisplayBase::updateTopic();
mouse_click_->updateTopic(topic_property_->getTopic());
mouse_click_->setImageTopic(topic_property_->getTopic());
}


Expand Down
3 changes: 1 addition & 2 deletions src/rviz/default_plugin/image_display.h
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,6 @@ public Q_SLOTS:

/* This is called by incomingMessage(). */
void processMessage(const sensor_msgs::Image::ConstPtr& msg) override;
void setTopic(const QString& topic, const QString& datatype) override;
void updateTopic() override;

Ogre::SceneManager* img_scene_manager_;
Expand All @@ -102,7 +101,7 @@ public Q_SLOTS:
IntProperty* median_buffer_size_property_;
bool got_float_image_;

boost::shared_ptr<MouseClick> mouse_click_;
MouseClick* mouse_click_;
};

} // namespace rviz
Expand Down
72 changes: 21 additions & 51 deletions src/rviz/image/mouse_click.cpp
Original file line number Diff line number Diff line change
@@ -1,52 +1,32 @@
#include "rviz/image/mouse_click.h"

#include <QWidget>
#include <ros/names.h>


namespace rviz
{
MouseClick::MouseClick(QObject* parent, const ros::NodeHandle& nh) : QObject(parent)
{
node_handle_.reset(new ros::NodeHandle());
*node_handle_ = nh;
has_dimensions_ = false;
is_topic_name_ok_ = false;
setParent(parent);
parent->installEventFilter(this);
}

MouseClick::~MouseClick()
{
}

void MouseClick::onInitialize()
MouseClick::MouseClick(QWidget* widget, const ros::NodeHandle& nh) : QObject(widget)
{
node_handle_ = nh;
have_dimensions_ = false;
topic_name_ok_ = false;
widget->installEventFilter(this);
}

void MouseClick::publish()
void MouseClick::enable()
{
if (is_topic_name_ok_)
{
publisher_.reset(new ros::Publisher(node_handle_->advertise<geometry_msgs::PointStamped>(topic_, 1)));
}
if (topic_name_ok_)
publisher_ = node_handle_.advertise<geometry_msgs::PointStamped>(topic_, 1);
}

void MouseClick::unpublish()
void MouseClick::disable()
{
publisher_.reset();
publisher_.shutdown();
}


bool MouseClick::eventFilter(QObject* obj, QEvent* event)
{
if (has_dimensions_ == false)
return false; // Cannot compute pixel coordinates.

if (is_topic_name_ok_ == false)
return false; // Cannot publish.


if (event->type() == QEvent::MouseButtonPress)
if (have_dimensions_ && publisher_.operator void*() && event->type() == QEvent::MouseButtonPress)
{
QMouseEvent* me = static_cast<QMouseEvent*>(event);
QPointF windowPos = me->windowPos();
Expand Down Expand Up @@ -82,7 +62,7 @@ bool MouseClick::eventFilter(QObject* obj, QEvent* event)
point_msg.header.stamp = ros::Time::now();
point_msg.point.x = pix_x;
point_msg.point.y = pix_y;
publisher_->publish(point_msg);
publisher_.publish(point_msg);
}
}
}
Expand All @@ -95,30 +75,20 @@ void MouseClick::setDimensions(int img_width, int img_height, int win_width, int
img_height_ = img_height;
win_width_ = win_width;
win_height_ = win_height;
has_dimensions_ = true;
have_dimensions_ = true;
}

void MouseClick::setTopic(const QString& image_topic)
void MouseClick::setImageTopic(const QString& topic)
{
// Build the click full topic name based on the image topic.
topic_ = image_topic.toStdString().append("/mouse_click");
disable();

// Build the click full topic name based on the image topic
topic_ = topic.toStdString().append("/mouse_click");

std::string error;
if (ros::names::validate((const std::string)topic_, error))
{
is_topic_name_ok_ = true;
}
else
{
is_topic_name_ok_ = false;
}
}
topic_name_ok_ = ros::names::validate(topic_, error);

void MouseClick::updateTopic(const QString& image_topic)
{
unpublish();
setTopic(image_topic);
publish();
enable();
}

} // namespace rviz
21 changes: 8 additions & 13 deletions src/rviz/image/mouse_click.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,28 +34,23 @@ namespace rviz
class RVIZ_EXPORT MouseClick : QObject
{
public:
MouseClick(QObject* parent, const ros::NodeHandle& nh);
~MouseClick();
MouseClick(QWidget* widget, const ros::NodeHandle& nh);

void onInitialize();

/** @brief ROS topic management. */
void publish();
void unpublish();
void enable();
void disable();

void setDimensions(int img_width, int img_height, int win_width, int win_height);
void setTopic(const QString& image_topic);
void updateTopic(const QString& image_topic);
void setImageTopic(const QString& topic);

private:
virtual bool eventFilter(QObject* obj, QEvent* event);

bool has_dimensions_;
bool have_dimensions_;
int img_width_, img_height_, win_width_, win_height_; // To assess if the clicks are inside the image.
boost::shared_ptr<ros::NodeHandle> node_handle_;
boost::shared_ptr<ros::Publisher> publisher_;
ros::NodeHandle node_handle_;
ros::Publisher publisher_;
std::string topic_;
bool is_topic_name_ok_;
bool topic_name_ok_;
};

} // namespace rviz
Expand Down

0 comments on commit 1a48217

Please sign in to comment.