diff --git a/atom_calibration/src/atom_calibration/calibration/visualization.py b/atom_calibration/src/atom_calibration/calibration/visualization.py index 24fdbdf9..e228a334 100644 --- a/atom_calibration/src/atom_calibration/calibration/visualization.py +++ b/atom_calibration/src/atom_calibration/calibration/visualization.py @@ -154,7 +154,7 @@ def setupVisualization(dataset, args, selected_collection_key): # graphics['ros']['tf_broadcaster'] = tf.TransformBroadcaster() graphics['ros']['tf_broadcaster'] = tf2_ros.TransformBroadcaster() - rospy.sleep(0.2) # Sleep a litle to make sure the time.now() returns a correct time. + rospy.sleep(0.2) # Sleep a little to make sure the time.now() returns a correct time. now = rospy.Time.now() graphics['ros']['publisher_models'] = rospy.Publisher('~robot_meshes', MarkerArray, queue_size=0, latch=True) diff --git a/atom_rviz_plugins/image_display_with_click/CMakeLists.txt b/atom_rviz_plugins/image_display_with_click/CMakeLists.txt index a8e9e502..b2db1258 100644 --- a/atom_rviz_plugins/image_display_with_click/CMakeLists.txt +++ b/atom_rviz_plugins/image_display_with_click/CMakeLists.txt @@ -61,6 +61,7 @@ link_directories(${catkin_LIBRARY_DIRS}) # source codes # set(HEADERS include/image_display_with_click/image_display_with_click.h + include/image_display_with_click/image_display_with_click_base.h include/image_display_with_click/mouse_watcher.h ) #qt5_wrap_ui(UIC_FILES @@ -70,12 +71,15 @@ set(HEADERS set(SOURCE_FILES src/plugin_load.cpp src/image_display_with_click.cpp + src/image_display_with_click_base.cpp src/mouse_watcher.cpp ) add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${HEADERS} ${UIC_FILES}) +# add_library(${PROJECT_NAME} ${SOURCE_FILES}) set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_include_directories(${PROJECT_NAME} PRIVATE "${OGRE_PREFIX_DIR}/include") +# target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 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 index e9da8e2a..e05a9081 100644 --- 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 @@ -22,6 +22,7 @@ #include "std_msgs/String.h" #include +#include namespace Ogre { @@ -30,10 +31,11 @@ namespace Ogre } // namespace Ogre using namespace std; +using namespace rviz; namespace atom_rviz { - class ImageDisplayWithClick : public rviz::ImageDisplayBase + class ImageDisplayWithClick : public atom_rviz::ImageDisplayWithClickBase { Q_OBJECT public: diff --git a/atom_rviz_plugins/image_display_with_click/include/image_display_with_click/image_display_with_click_base.h b/atom_rviz_plugins/image_display_with_click/include/image_display_with_click/image_display_with_click_base.h new file mode 100644 index 00000000..30a94862 --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/include/image_display_with_click/image_display_with_click_base.h @@ -0,0 +1,112 @@ +#ifndef IMAGE_DISPLAY_WITH_CLICK_BASE_H +#define IMAGE_DISPLAY_WITH_CLICK_BASE_H + +#include + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include + +#include +#include + +#include "rviz/display_context.h" +#include "rviz/frame_manager.h" +#include "rviz/properties/ros_topic_property.h" +#include "rviz/properties/enum_property.h" +#include "rviz/properties/int_property.h" + +#include "rviz/display.h" +#include "rviz/rviz_export.h" +#endif + +using namespace rviz; + +namespace atom_rviz +{ +/** @brief Display subclass for subscribing and displaying to image messages. + * + * This class brings together some common things used for subscribing and displaying image messages in + * Display + * types. It has a tf2_ros::MessageFilter and image_tranport::SubscriberFilter to filter incoming image + * messages, and + * it handles subscribing and unsubscribing when the display is + * enabled or disabled. */ + +class RVIZ_EXPORT ImageDisplayWithClickBase : public Display +{ + Q_OBJECT +public: + /** @brief Constructor. */ + ImageDisplayWithClickBase(); + ~ImageDisplayWithClickBase() override; + + void setTopic(const QString& topic, const QString& datatype) override; + +protected Q_SLOTS: + /** @brief Update topic and resubscribe */ + virtual void updateTopic(); + + /** @brief Update queue size of tf filter */ + virtual void updateQueueSize(); + + /** @brief Fill list of available and working transport options */ + void fillTransportOptionList(EnumProperty* property); + +protected: + void onInitialize() override; + + /** @brief Reset display. */ + void reset() override; + + /** @brief Enabling TF filtering by defining a target frame. */ + void enableTFFilter(std::string& targetFrame) + { + targetFrame_ = targetFrame; + reset(); + } + + /** @brief ROS topic management. */ + virtual void subscribe(); + virtual void unsubscribe(); + + void fixedFrameChanged() override; + + /** @brief Incoming message callback. Checks if the message pointer + * is valid, increments messages_received_, then calls + * processMessage(). */ + void incomingMessage(const sensor_msgs::Image::ConstPtr& msg); + + /** @brief Callback for messages, whose frame_id cannot resolved */ + void failedMessage(const sensor_msgs::Image::ConstPtr& msg, tf2_ros::FilterFailureReason reason); + + /** @brief Implement this to process the contents of a message. + * + * This is called by incomingMessage(). */ + virtual void processMessage(const sensor_msgs::Image::ConstPtr& msg) = 0; + + void scanForTransportSubscriberPlugins(); + + boost::scoped_ptr it_; + boost::shared_ptr sub_; + boost::shared_ptr > tf_filter_; + + std::string targetFrame_; + + uint32_t messages_received_; + + RosTopicProperty* topic_property_; + EnumProperty* transport_property_; + IntProperty* queue_size_property_; + + std::string transport_; + + std::set transport_plugin_types_; + + BoolProperty* unreliable_property_; +}; + +} // end namespace rviz + +#endif // IMAGE_DISPLAY_BASE_H \ No newline at end of file diff --git a/atom_rviz_plugins/image_display_with_click/package.xml b/atom_rviz_plugins/image_display_with_click/package.xml index 27eb8075..a88ef833 100644 --- a/atom_rviz_plugins/image_display_with_click/package.xml +++ b/atom_rviz_plugins/image_display_with_click/package.xml @@ -51,7 +51,7 @@ class_loader pluginlib - rviz + qtbase5-dev catkin atom_msgs @@ -67,6 +67,8 @@ rospy std_msgs + rviz + rviz 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 index 1f71a45c..4ecf6fa6 100644 --- 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 @@ -27,7 +27,7 @@ using namespace rviz; namespace atom_rviz { - ImageDisplayWithClick::ImageDisplayWithClick() : ImageDisplayBase(), texture_() + ImageDisplayWithClick::ImageDisplayWithClick() : ImageDisplayWithClickBase(), texture_() { cout << "Called ImageDisplay::ImageDisplay()" << endl; normalize_property_ = new BoolProperty( @@ -56,9 +56,12 @@ namespace atom_rviz { cout << "Called ImageDisplay::onInitialize()" << endl; - ImageDisplayBase::onInitialize(); + ImageDisplayWithClickBase::onInitialize(); { - static uint32_t count = 0; + static uint32_t count = 10000; //NOTE this must start with a different number than 0, otherwise it will + // collide with the scene managers created by ImageDisplay + // https://github.com/lardemua/atom/issues/378#issuecomment-1091711581 + std::stringstream ss; ss << "ImageDisplay" << count++; img_scene_manager_ = Ogre::Root::getSingleton().createSceneManager(Ogre::ST_GENERIC, ss.str()); @@ -136,7 +139,7 @@ namespace atom_rviz void ImageDisplayWithClick::onDisable() { render_panel_->getRenderWindow()->setActive(false); - ImageDisplayBase::unsubscribe(); + ImageDisplayWithClickBase::unsubscribe(); reset(); } @@ -210,7 +213,7 @@ namespace atom_rviz void ImageDisplayWithClick::reset() { - ImageDisplayBase::reset(); + ImageDisplayWithClickBase::reset(); texture_.clear(); render_panel_->getCamera()->setPosition(Ogre::Vector3(999999, 999999, 999999)); } diff --git a/atom_rviz_plugins/image_display_with_click/src/image_display_with_click_base.cpp b/atom_rviz_plugins/image_display_with_click/src/image_display_with_click_base.cpp new file mode 100644 index 00000000..978fc043 --- /dev/null +++ b/atom_rviz_plugins/image_display_with_click/src/image_display_with_click_base.cpp @@ -0,0 +1,281 @@ +#include +#include +#include + +#include + +#include + +#include + +//#include +#include + +namespace atom_rviz +{ +ImageDisplayWithClickBase::ImageDisplayWithClickBase() : Display(), sub_(), tf_filter_(), messages_received_(0) +{ + topic_property_ = + new RosTopicProperty("Image Topic", "", + QString::fromStdString(ros::message_traits::datatype()), + "sensor_msgs::Image topic to subscribe to.", this, SLOT(updateTopic())); + + transport_property_ = new EnumProperty("Transport Hint", "raw", "Preferred method of sending images.", + this, SLOT(updateTopic())); + + connect(transport_property_, SIGNAL(requestOptions(EnumProperty*)), this, + SLOT(fillTransportOptionList(EnumProperty*))); + + queue_size_property_ = + new IntProperty("Queue Size", 2, + "Advanced: set the size of the incoming message queue. Increasing this " + "is useful if your incoming TF data is delayed significantly from your" + " image data, but it can greatly increase memory usage if the messages are big.", + this, SLOT(updateQueueSize())); + queue_size_property_->setMin(1); + + transport_property_->setStdString("raw"); + + unreliable_property_ = + new BoolProperty("Unreliable", false, "Prefer UDP topic transport", this, SLOT(updateTopic())); +} + +ImageDisplayWithClickBase::~ImageDisplayWithClickBase() +{ + ImageDisplayWithClickBase::unsubscribe(); +} + +void ImageDisplayWithClickBase::onInitialize() +{ + it_.reset(new image_transport::ImageTransport(update_nh_)); + scanForTransportSubscriberPlugins(); +} + +void ImageDisplayWithClickBase::setTopic(const QString& topic, const QString& datatype) +{ + if (datatype == ros::message_traits::datatype()) + { + transport_property_->setStdString("raw"); + topic_property_->setString(topic); + } + else + { + int index = topic.lastIndexOf("/"); + if (index == -1) + { + ROS_WARN("ImageDisplayBase::setTopic() Invalid topic name: %s", topic.toStdString().c_str()); + return; + } + QString transport = topic.mid(index + 1); + QString base_topic = topic.mid(0, index); + + transport_property_->setString(transport); + topic_property_->setString(base_topic); + } +} + + +void ImageDisplayWithClickBase::incomingMessage(const sensor_msgs::Image::ConstPtr& msg) +{ + if (!msg || context_->getFrameManager()->getPause()) + { + return; + } + + ++messages_received_; + setStatus(StatusProperty::Ok, "Image", QString::number(messages_received_) + " images received"); + + emitTimeSignal(msg->header.stamp); + + processMessage(msg); +} + + +void ImageDisplayWithClickBase::failedMessage(const sensor_msgs::Image::ConstPtr& msg, + tf2_ros::FilterFailureReason reason) +{ + setStatusStd(StatusProperty::Error, "Image", + context_->getFrameManager()->discoverFailureReason(msg->header.frame_id, + msg->header.stamp, "", reason)); +} + + +void ImageDisplayWithClickBase::reset() +{ + Display::reset(); + if (tf_filter_) + { + tf_filter_->clear(); + } + + messages_received_ = 0; + setStatus(StatusProperty::Warn, "Image", "No Image received"); +} + +void ImageDisplayWithClickBase::updateQueueSize() +{ + uint32_t size = queue_size_property_->getInt(); + if (tf_filter_) + { + tf_filter_->setQueueSize(size); + subscribe(); + } +} + +void ImageDisplayWithClickBase::subscribe() +{ + if (!isEnabled()) + { + return; + } + + try + { + tf_filter_.reset(); + + sub_.reset(new image_transport::SubscriberFilter()); + + if (!topic_property_->getTopicStd().empty() && !transport_property_->getStdString().empty()) + { + // Determine UDP vs TCP transport for user selection. + if (unreliable_property_->getBool()) + { + sub_->subscribe(*it_, topic_property_->getTopicStd(), (uint32_t)queue_size_property_->getInt(), + image_transport::TransportHints(transport_property_->getStdString(), + ros::TransportHints().unreliable())); + } + else + { + sub_->subscribe(*it_, topic_property_->getTopicStd(), (uint32_t)queue_size_property_->getInt(), + image_transport::TransportHints(transport_property_->getStdString())); + } + + + if (targetFrame_.empty()) + { + sub_->registerCallback( + boost::bind(&ImageDisplayWithClickBase::incomingMessage, this, boost::placeholders::_1)); + } + else + { + tf_filter_.reset(new tf2_ros::MessageFilter( + *sub_, *context_->getTF2BufferPtr(), targetFrame_, queue_size_property_->getInt(), + update_nh_)); + tf_filter_->registerCallback( + boost::bind(&ImageDisplayWithClickBase::incomingMessage, this, boost::placeholders::_1)); + tf_filter_->registerFailureCallback(boost::bind( + &ImageDisplayWithClickBase::failedMessage, this, boost::placeholders::_1, boost::placeholders::_2)); + } + } + setStatus(StatusProperty::Ok, "Topic", "OK"); + } + catch (ros::Exception& e) + { + setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what()); + } + catch (image_transport::Exception& e) + { + setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what()); + } +} + +void ImageDisplayWithClickBase::unsubscribe() +{ + tf_filter_.reset(); + sub_.reset(); +} + +void ImageDisplayWithClickBase::fixedFrameChanged() +{ + if (tf_filter_) + { + tf_filter_->setTargetFrame(fixed_frame_.toStdString()); + reset(); + } +} + +void ImageDisplayWithClickBase::scanForTransportSubscriberPlugins() +{ + pluginlib::ClassLoader sub_loader( + "image_transport", "image_transport::SubscriberPlugin"); + + BOOST_FOREACH (const std::string& lookup_name, sub_loader.getDeclaredClasses()) + { + // lookup_name is formatted as "pkg/transport_sub", for instance + // "image_transport/compressed_sub" for the "compressed" + // transport. This code removes the "_sub" from the tail and + // everything up to and including the "/" from the head, leaving + // "compressed" (for example) in transport_name. + std::string transport_name = boost::erase_last_copy(lookup_name, "_sub"); + transport_name = transport_name.substr(lookup_name.find('/') + 1); + + // If the plugin loads without throwing an exception, add its + // transport name to the list of valid plugins, otherwise ignore + // it. + try + { + boost::shared_ptr sub = sub_loader.createInstance(lookup_name); + transport_plugin_types_.insert(transport_name); + } + catch (const pluginlib::LibraryLoadException& e) + { + } + catch (const pluginlib::CreateClassException& e) + { + } + } +} + +void ImageDisplayWithClickBase::updateTopic() +{ + unsubscribe(); + reset(); + subscribe(); + context_->queueRender(); +} + +void ImageDisplayWithClickBase::fillTransportOptionList(EnumProperty* property) +{ + property->clearOptions(); + + std::vector choices; + + choices.push_back("raw"); + + // Loop over all current ROS topic names + ros::master::V_TopicInfo topics; + ros::master::getTopics(topics); + ros::master::V_TopicInfo::iterator it = topics.begin(); + ros::master::V_TopicInfo::iterator end = topics.end(); + for (; it != end; ++it) + { + // If the beginning of this topic name is the same as topic_, + // and the whole string is not the same, + // and the next character is / + // and there are no further slashes from there to the end, + // then consider this a possible transport topic. + const ros::master::TopicInfo& ti = *it; + const std::string& topic_name = ti.name; + const std::string& topic = topic_property_->getStdString(); + + if (topic_name.find(topic) == 0 && topic_name != topic && topic_name[topic.size()] == '/' && + topic_name.find('/', topic.size() + 1) == std::string::npos) + { + std::string transport_type = topic_name.substr(topic.size() + 1); + + // If the transport type string found above is in the set of + // supported transport type plugins, add it to the list. + if (transport_plugin_types_.find(transport_type) != transport_plugin_types_.end()) + { + choices.push_back(transport_type); + } + } + } + + for (size_t i = 0; i < choices.size(); i++) + { + property->addOptionStd(choices[i]); + } +} + +} // end namespace atom_rviz \ No newline at end of file