From 95b2e04ba9527ee6d1f2e55b100f00d3a280128a Mon Sep 17 00:00:00 2001 From: Miguel Oliveira Date: Fri, 1 Apr 2022 18:51:24 +0100 Subject: [PATCH] #378 Basic functionality working. Will need more work but we can already published the clicked points. --- .../image_display_with_click/CMakeLists.txt | 93 +++++++ .../image_display_with_click.h | 85 +++++++ .../image_display_with_click/mouse_watcher.h | 50 ++++ .../old_CMakeLists.txt | 206 +++++++++++++++ .../image_display_with_click/package.xml | 73 ++++++ .../plugin_description.xml | 8 + .../src/image_display_with_click.cpp | 238 ++++++++++++++++++ .../src/mouse_watcher.cpp | 95 +++++++ .../src/plugin_load.cpp | 4 + 9 files changed, 852 insertions(+) create mode 100644 atom_rviz_plugins/image_display_with_click/CMakeLists.txt create mode 100644 atom_rviz_plugins/image_display_with_click/include/image_display_with_click/image_display_with_click.h create mode 100644 atom_rviz_plugins/image_display_with_click/include/image_display_with_click/mouse_watcher.h create mode 100644 atom_rviz_plugins/image_display_with_click/old_CMakeLists.txt create mode 100644 atom_rviz_plugins/image_display_with_click/package.xml create mode 100644 atom_rviz_plugins/image_display_with_click/plugin_description.xml create mode 100644 atom_rviz_plugins/image_display_with_click/src/image_display_with_click.cpp create mode 100644 atom_rviz_plugins/image_display_with_click/src/mouse_watcher.cpp create mode 100644 atom_rviz_plugins/image_display_with_click/src/plugin_load.cpp diff --git a/atom_rviz_plugins/image_display_with_click/CMakeLists.txt b/atom_rviz_plugins/image_display_with_click/CMakeLists.txt new file mode 100644 index 00000000..a8e9e502 --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.5) +project(image_display_with_click) + +################### +# compile options # +################### +set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -fno-asm -fPIC") +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -g -O0") + + +# Qt settings # +find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) +#find_package(QCustomPlot) + +macro(qt_wrap_ui) + qt5_wrap_ui(${ARGN}) +endmacro() +include_directories(${Qt5Core_INCLUDE_DIRS}) +include_directories(${Qt5Widgets_INCLUDE_DIRS}) +add_definitions(-DQT_NO_KEYWORDS) +# cmake settings # +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTORCC ON) + +# catkin # +set(DEPEND_ROS_PKGS + class_loader + pluginlib + roscpp + rviz + std_msgs + atom_msgs + ) +find_package(catkin REQUIRED + COMPONENTS + ${DEPEND_ROS_PKGS} + ) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + ${DEPEND_ROS_PKGS} + DEPENDS + ) + +######### +# Build # +######### +## add include directory ## +include_directories( + include + ${catkin_INCLUDE_DIRS} + ) +## add library path ## +link_directories(${catkin_LIBRARY_DIRS}) + +# source codes # +set(HEADERS + include/image_display_with_click/image_display_with_click.h + include/image_display_with_click/mouse_watcher.h + ) +#qt5_wrap_ui(UIC_FILES +# src/ui/calibration_panel.ui +# ) + +set(SOURCE_FILES + src/plugin_load.cpp + src/image_display_with_click.cpp + src/mouse_watcher.cpp + ) + +add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${HEADERS} ${UIC_FILES}) +set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_include_directories(${PROJECT_NAME} PRIVATE "${OGRE_PREFIX_DIR}/include") + + + +########### +# Install # +########### +install(FILES plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +install(DIRECTORY icons DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY include/ DESTINATION include) +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) diff --git a/atom_rviz_plugins/image_display_with_click/include/image_display_with_click/image_display_with_click.h b/atom_rviz_plugins/image_display_with_click/include/image_display_with_click/image_display_with_click.h new file mode 100644 index 00000000..e9da8e2a --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/include/image_display_with_click/image_display_with_click.h @@ -0,0 +1,85 @@ +#ifndef IMAGE_DISPLAY_WITH_CLICK_H +#define IMAGE_DISPLAY_WITH_CLICK_H + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include "../../../../../../../usr/include/x86_64-linux-gnu/qt5/QtCore/QObject" +#include "../../../../../../../usr/include/OGRE/OgreMaterial.h" +#include "../../../../../../../usr/include/OGRE/OgreRenderTargetListener.h" +#include "../../../../../../../usr/include/OGRE/OgreSharedPtr.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/image/image_display_base.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/image/ros_image_texture.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/render_panel.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/properties/bool_property.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/properties/float_property.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/properties/int_property.h" +#endif + +#include +#include + +#include "ros/ros.h" +#include "geometry_msgs/PointStamped.h" +#include "std_msgs/String.h" + +#include + +namespace Ogre +{ + class SceneNode; + class Rectangle2D; +} // namespace Ogre + +using namespace std; + +namespace atom_rviz +{ + class ImageDisplayWithClick : public rviz::ImageDisplayBase + { + Q_OBJECT + public: + ImageDisplayWithClick(); + ~ImageDisplayWithClick() override; + + // Overrides from Display + void onInitialize() override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + + public Q_SLOTS: + virtual void updateNormalizeOptions(); + + + + protected: + // overrides from Display + void onEnable() override; + void onDisable() override; + + /* This is called by incomingMessage(). */ + void processMessage(const sensor_msgs::Image::ConstPtr& msg) override; + + Ogre::SceneManager* img_scene_manager_; + + rviz::ROSImageTexture texture_; + + rviz::RenderPanel* render_panel_; + + + private: + Ogre::SceneNode* img_scene_node_; + Ogre::Rectangle2D* screen_rect_; + Ogre::MaterialPtr material_; + + BoolProperty* normalize_property_; + rviz::FloatProperty* min_property_; + rviz::FloatProperty* max_property_; + rviz::IntProperty* median_buffer_size_property_; + bool got_float_image_; + + MouseWatcher* mouse_watcher; + }; + +} // namespace rviz + + +#endif diff --git a/atom_rviz_plugins/image_display_with_click/include/image_display_with_click/mouse_watcher.h b/atom_rviz_plugins/image_display_with_click/include/image_display_with_click/mouse_watcher.h new file mode 100644 index 00000000..e15ec556 --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/include/image_display_with_click/mouse_watcher.h @@ -0,0 +1,50 @@ +#ifndef MOUSE_WATCHER_H +#define MOUSE_WATCHER_H + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include "../../../../../../../usr/include/x86_64-linux-gnu/qt5/QtCore/QObject" +#include "../../../../../../../usr/include/OGRE/OgreMaterial.h" +#include "../../../../../../../usr/include/OGRE/OgreRenderTargetListener.h" +#include "../../../../../../../usr/include/OGRE/OgreSharedPtr.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/image/image_display_base.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/image/ros_image_texture.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/render_panel.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/properties/bool_property.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/properties/float_property.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/properties/int_property.h" +#endif + +#include +#include + +#include + +#include "ros/ros.h" +#include "geometry_msgs/PointStamped.h" +#include "std_msgs/String.h" + +using namespace std; + +namespace atom_rviz +{ +class MouseWatcher : public QObject +{ +public: + MouseWatcher(QWidget * parent); + virtual bool eventFilter(QObject * obj, QEvent * event); + void setDimensions(int _img_width, int _img_height, int _win_width, int _win_height); + void setTopic(string image_topic); + +private: + string image_click_topic; + bool has_dimensions; + int img_width; + int img_height; + int win_width; + int win_height; + ros::Publisher* mouse_event_pub; + ros::NodeHandle* node_handle; +}; + +} // namespace rviz +#endif diff --git a/atom_rviz_plugins/image_display_with_click/old_CMakeLists.txt b/atom_rviz_plugins/image_display_with_click/old_CMakeLists.txt new file mode 100644 index 00000000..ba931336 --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/old_CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 3.0.2) +project(image_display_with_click) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES image_display_with_click +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/image_display_with_click.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/image_display_with_click_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_image_display_with_click.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/atom_rviz_plugins/image_display_with_click/package.xml b/atom_rviz_plugins/image_display_with_click/package.xml new file mode 100644 index 00000000..27eb8075 --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/package.xml @@ -0,0 +1,73 @@ + + + image_display_with_click + 0.0.0 + The image_display_with_click package + + + + + mike + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + class_loader + pluginlib + rviz + qtbase5-dev + catkin + atom_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + atom_msgs + roscpp + rospy + std_msgs + + + + + diff --git a/atom_rviz_plugins/image_display_with_click/plugin_description.xml b/atom_rviz_plugins/image_display_with_click/plugin_description.xml new file mode 100644 index 00000000..34546e8e --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/plugin_description.xml @@ -0,0 +1,8 @@ + + + + + Image display with mouse clicking functionality. + + + diff --git a/atom_rviz_plugins/image_display_with_click/src/image_display_with_click.cpp b/atom_rviz_plugins/image_display_with_click/src/image_display_with_click.cpp new file mode 100644 index 00000000..1f71a45c --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/src/image_display_with_click.cpp @@ -0,0 +1,238 @@ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace rviz; + +namespace atom_rviz +{ + ImageDisplayWithClick::ImageDisplayWithClick() : ImageDisplayBase(), texture_() + { + cout << "Called ImageDisplay::ImageDisplay()" << endl; + 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())); + + median_buffer_size_property_ = + new IntProperty("Median window", 5, "Window size for median filter used for computin min/max.", + this, SLOT(updateNormalizeOptions())); + + got_float_image_ = false; + + QString ss = topic_property_->getTopic(); + cout << "constructor topic is " << topic_property_->getTopicStd() << endl; + + } + + void ImageDisplayWithClick::onInitialize() + { + + cout << "Called ImageDisplay::onInitialize()" << endl; + 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_node_ = img_scene_manager_->getRootSceneNode()->createChildSceneNode(); + + { + static int count = 0; + std::stringstream ss; + ss << "ImageDisplayObject" << count++; + + screen_rect_ = new Ogre::Rectangle2D(true); + screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1); + 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_->setDepthWriteEnabled(false); + material_->setReceiveShadows(false); + material_->setDepthCheckEnabled(false); + + material_->getTechnique(0)->setLightingEnabled(false); + Ogre::TextureUnitState* tu = material_->getTechnique(0)->getPass(0)->createTextureUnitState(); + tu->setTextureName(texture_.getTexture()->getName()); + tu->setTextureFiltering(Ogre::TFO_NONE); + + material_->setCullingMode(Ogre::CULL_NONE); + Ogre::AxisAlignedBox aabInf; + aabInf.setInfinite(); + screen_rect_->setBoundingBox(aabInf); + setMaterial(*screen_rect_, material_); + img_scene_node_->attachObject(screen_rect_); + } + + render_panel_ = new RenderPanel(); + render_panel_->getRenderWindow()->setAutoUpdated(false); + render_panel_->getRenderWindow()->setActive(false); + + render_panel_->resize(640, 480); + render_panel_->initialize(img_scene_manager_, context_); + + setAssociatedWidget(render_panel_); + + render_panel_->setAutoRender(false); + render_panel_->setOverlaysEnabled(false); + render_panel_->getCamera()->setNearClipDistance(0.01f); + + mouse_watcher = new MouseWatcher(render_panel_); + render_panel_->installEventFilter(mouse_watcher); + + updateNormalizeOptions(); + + cout << "OnInitialize topic is " << topic_property_->getTopicStd() << endl; + } + + ImageDisplayWithClick::~ImageDisplayWithClick() + { + if (initialized()) + { + delete render_panel_; + delete screen_rect_; + removeAndDestroyChildNode(img_scene_node_->getParentSceneNode(), img_scene_node_); + } + } + + void ImageDisplayWithClick::onEnable() + { + ImageDisplayWithClick::subscribe(); + render_panel_->getRenderWindow()->setActive(true); + } + + void ImageDisplayWithClick::onDisable() + { + render_panel_->getRenderWindow()->setActive(false); + ImageDisplayBase::unsubscribe(); + reset(); + } + + void ImageDisplayWithClick::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 + { + normalize_property_->setHidden(true); + min_property_->setHidden(true); + max_property_->setHidden(true); + median_buffer_size_property_->setHidden(true); + } + } + + void ImageDisplayWithClick::update(float wall_dt, float ros_dt) + { + Q_UNUSED(wall_dt) + Q_UNUSED(ros_dt) + try + { + texture_.update(); + + // 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) + { + 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); + } + } + + render_panel_->getRenderWindow()->update(); + mouse_watcher->setDimensions(img_width, img_height, win_width, win_height); + } + catch (UnsupportedImageEncoding& e) + { + setStatus(StatusProperty::Error, "Image", e.what()); + } + + // Set new topic name to assess whether the publisher should be reconfigured. + // TODO this should be done only when the topic is chenged, not always. + mouse_watcher->setTopic(topic_property_->getTopicStd()); + } + + void ImageDisplayWithClick::reset() + { + ImageDisplayBase::reset(); + texture_.clear(); + render_panel_->getCamera()->setPosition(Ogre::Vector3(999999, 999999, 999999)); + } + +/* This is called by incomingMessage(). */ + void ImageDisplayWithClick::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_) + { + got_float_image_ = got_float_image; + updateNormalizeOptions(); + } + texture_.addMessage(msg); + } + +} // namespace atom_rviz + + +#include +PLUGINLIB_EXPORT_CLASS(atom_rviz::ImageDisplayWithClick, rviz::Display) diff --git a/atom_rviz_plugins/image_display_with_click/src/mouse_watcher.cpp b/atom_rviz_plugins/image_display_with_click/src/mouse_watcher.cpp new file mode 100644 index 00000000..ec890381 --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/src/mouse_watcher.cpp @@ -0,0 +1,95 @@ +#include + +using namespace std; + +namespace atom_rviz +{ + +MouseWatcher::MouseWatcher(QWidget * parent) : QObject(parent) { + has_dimensions = false; + node_handle = new ros::NodeHandle(); + mouse_event_pub = new ros::Publisher(); + } + + +bool MouseWatcher::eventFilter(QObject * obj, QEvent * event) + { + if (event->type() == QEvent::MouseButtonPress) + { + QMouseEvent * me = static_cast(event); + QPointF windowPos = me->windowPos(); + printf("User clicked mouse at %f,%f\n", windowPos.x(), windowPos.y()); + cout << "img_width = " << img_width << endl; + cout << "img_height = " << img_height << endl; + cout << "win_width = " << win_width << endl; + cout << "win_height = " << win_height << endl; + + if (img_width != 0 && img_height != 0 && win_width != 0 && win_height != 0) + { + float img_aspect = float(img_width) / float(img_height); + float win_aspect = float(win_width) / float(win_height); + cout << "img_aspect = " << img_aspect << endl; + cout << "win_aspect = " << win_aspect << endl; + + int pix_x=-1; + int pix_y=-1; + if (img_aspect > win_aspect) // case where the window is taller than the image + { + cout << "black bars on top!" << endl; + pix_x = int(float(windowPos.x()) / float(win_width) * float(img_width)); + + int resized_img_height = int(float(win_width) / float(img_width) * float(img_height)); + int bias = int( ( float(win_height) - float(resized_img_height) )/2.0); + pix_y = (float(windowPos.y()) - bias) / float(resized_img_height) * float(img_height); + cout << "pix_x = " << pix_x << endl; + cout << "resized_img_height = " << resized_img_height << endl; + cout << "bias = " << bias << endl; + cout << "pix_y = " << pix_y << endl; + } + else // case where the window is wider than the image + { + cout << "black bars on side!" << endl; + // TODO add the computation of pix_x and pix_y for this case + } + + // Check if clicked point is inside the image, publish if so + if (pix_x > 0 && pix_x < img_width && pix_y > 0 && pix_y < img_height) + { + cout << "You clicked inside the image. I should publish ..." << endl; + geometry_msgs::PointStamped point_msgs; + point_msgs.header.stamp = ros::Time::now(); + point_msgs.point.x = pix_x; + point_msgs.point.y = pix_y; + mouse_event_pub->publish(point_msgs); + } + } + } + return QObject::eventFilter(obj, event); + } + + void MouseWatcher::setDimensions(int _img_width, int _img_height, int _win_width, int _win_height) + { + img_width = _img_width; + img_height = _img_height; + win_width = _win_width; + win_height = _win_height; + has_dimensions = true; + } + + void MouseWatcher::setTopic(string image_topic) + { + string tmp_topic = image_topic + "/click"; // build the click full topic name based on the image topic + + if (tmp_topic.compare(image_click_topic) != 0) // if topic changed, reconfigure the publisher + { + image_click_topic = tmp_topic; // set new topic + cout << "Changed pub topic, reconfiguring to publish on topic " << image_click_topic << endl; + + mouse_event_pub->shutdown(); // shutdown current publisher + delete mouse_event_pub; // delete the mem allocation (to avoid mem leaks) + // reconfigure new publisher + mouse_event_pub = new ros::Publisher(node_handle->advertise(image_click_topic, 1000)); + } + } + +} // atom_rviz diff --git a/atom_rviz_plugins/image_display_with_click/src/plugin_load.cpp b/atom_rviz_plugins/image_display_with_click/src/plugin_load.cpp new file mode 100644 index 00000000..8fbe18bb --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/src/plugin_load.cpp @@ -0,0 +1,4 @@ +#include "include/image_display_with_click/image_display_with_click.h" +#include + +PLUGINLIB_EXPORT_CLASS(atom_rviz::ImageDisplayWithClick, rviz::Display ) \ No newline at end of file