diff --git a/src/rviz/default_plugin/depth_cloud_display.cpp b/src/rviz/default_plugin/depth_cloud_display.cpp index 5e7d89c847..75a8a20622 100644 --- a/src/rviz/default_plugin/depth_cloud_display.cpp +++ b/src/rviz/default_plugin/depth_cloud_display.cpp @@ -159,10 +159,6 @@ void DepthCloudDisplay::onInitialize() updateUseAutoSize(); updateUseOcclusionCompensation(); - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - threaded_nh_.setCallbackQueue(pointcloud_common_->getCallbackQueue()); - // Scan for available transport plugins scanForTransportSubscriberPlugins(); diff --git a/src/rviz/default_plugin/fluid_pressure_display.cpp b/src/rviz/default_plugin/fluid_pressure_display.cpp index 4ff5e4007a..1b9e3a6ed2 100644 --- a/src/rviz/default_plugin/fluid_pressure_display.cpp +++ b/src/rviz/default_plugin/fluid_pressure_display.cpp @@ -52,10 +52,6 @@ FluidPressureDisplay::FluidPressureDisplay() : point_cloud_common_(new PointClou " Increasing this is useful if your incoming TF data is delayed significantly " "from your FluidPressure data, but it can greatly increase memory usage if the messages are big.", this, SLOT(updateQueueSize())); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue(point_cloud_common_->getCallbackQueue()); } FluidPressureDisplay::~FluidPressureDisplay() @@ -65,6 +61,9 @@ FluidPressureDisplay::~FluidPressureDisplay() void FluidPressureDisplay::onInitialize() { + // Use the threaded queue for processing of incoming messages + update_nh_.setCallbackQueue(context_->getThreadedQueue()); + MFDClass::onInitialize(); point_cloud_common_->initialize(context_, scene_node_); diff --git a/src/rviz/default_plugin/illuminance_display.cpp b/src/rviz/default_plugin/illuminance_display.cpp index fc5eba9d70..161ed962f4 100644 --- a/src/rviz/default_plugin/illuminance_display.cpp +++ b/src/rviz/default_plugin/illuminance_display.cpp @@ -52,10 +52,6 @@ IlluminanceDisplay::IlluminanceDisplay() : point_cloud_common_(new PointCloudCom " Increasing this is useful if your incoming TF data is delayed significantly " "from your Illuminance data, but it can greatly increase memory usage if the messages are big.", this, SLOT(updateQueueSize())); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue(point_cloud_common_->getCallbackQueue()); } IlluminanceDisplay::~IlluminanceDisplay() @@ -65,6 +61,9 @@ IlluminanceDisplay::~IlluminanceDisplay() void IlluminanceDisplay::onInitialize() { + // Use the threaded queue for processing of incoming messages + update_nh_.setCallbackQueue(context_->getThreadedQueue()); + MFDClass::onInitialize(); point_cloud_common_->initialize(context_, scene_node_); diff --git a/src/rviz/default_plugin/laser_scan_display.cpp b/src/rviz/default_plugin/laser_scan_display.cpp index 5ebe92e41a..166123955a 100644 --- a/src/rviz/default_plugin/laser_scan_display.cpp +++ b/src/rviz/default_plugin/laser_scan_display.cpp @@ -54,10 +54,6 @@ LaserScanDisplay::LaserScanDisplay() " Increasing this is useful if your incoming TF data is delayed significantly " "from your LaserScan data, but it can greatly increase memory usage if the messages are big.", this, SLOT(updateQueueSize())); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue(point_cloud_common_->getCallbackQueue()); } LaserScanDisplay::~LaserScanDisplay() @@ -68,6 +64,9 @@ LaserScanDisplay::~LaserScanDisplay() void LaserScanDisplay::onInitialize() { + // Use the threaded queue for processing of incoming messages + update_nh_.setCallbackQueue(context_->getThreadedQueue()); + MFDClass::onInitialize(); point_cloud_common_->initialize(context_, scene_node_); } diff --git a/src/rviz/default_plugin/point_cloud2_display.cpp b/src/rviz/default_plugin/point_cloud2_display.cpp index 398bf56225..4bcb48283b 100644 --- a/src/rviz/default_plugin/point_cloud2_display.cpp +++ b/src/rviz/default_plugin/point_cloud2_display.cpp @@ -52,10 +52,6 @@ PointCloud2Display::PointCloud2Display() : point_cloud_common_(new PointCloudCom " Increasing this is useful if your incoming TF data is delayed significantly " "from your PointCloud2 data, but it can greatly increase memory usage if the messages are big.", this, SLOT(updateQueueSize())); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue(point_cloud_common_->getCallbackQueue()); } PointCloud2Display::~PointCloud2Display() @@ -65,6 +61,9 @@ PointCloud2Display::~PointCloud2Display() void PointCloud2Display::onInitialize() { + // Use the threaded queue for processing of incoming messages + update_nh_.setCallbackQueue(context_->getThreadedQueue()); + MFDClass::onInitialize(); point_cloud_common_->initialize(context_, scene_node_); } diff --git a/src/rviz/default_plugin/point_cloud_common.cpp b/src/rviz/default_plugin/point_cloud_common.cpp index 55c69cbb78..441b7cafa2 100644 --- a/src/rviz/default_plugin/point_cloud_common.cpp +++ b/src/rviz/default_plugin/point_cloud_common.cpp @@ -307,7 +307,6 @@ void PointCloudCommon::CloudInfo::clear() PointCloudCommon::PointCloudCommon(Display* display) : auto_size_(false) - , spinner_(1, &cbqueue_) , new_xyz_transformer_(false) , new_color_transformer_(false) , needs_retransform_(false) @@ -377,18 +376,11 @@ void PointCloudCommon::initialize(DisplayContext* context, Ogre::SceneNode* scen updateBillboardSize(); updateAlpha(); updateSelectable(); - - spinner_.start(); } PointCloudCommon::~PointCloudCommon() { - spinner_.stop(); - - if (transformer_class_loader_) - { - delete transformer_class_loader_; - } + delete transformer_class_loader_; } void PointCloudCommon::loadTransformers() diff --git a/src/rviz/default_plugin/point_cloud_common.h b/src/rviz/default_plugin/point_cloud_common.h index 2033acc8dd..173a6b41f5 100644 --- a/src/rviz/default_plugin/point_cloud_common.h +++ b/src/rviz/default_plugin/point_cloud_common.h @@ -42,9 +42,6 @@ #include #include -#include -#include - #include #include @@ -130,11 +127,6 @@ class PointCloudCommon : public QObject void addMessage(const sensor_msgs::PointCloudConstPtr& cloud); void addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud); - ros::CallbackQueueInterface* getCallbackQueue() - { - return &cbqueue_; - } - Display* getDisplay() { return display_; @@ -187,9 +179,6 @@ private Q_SLOTS: void setPropertiesHidden(const QList& props, bool hide); void fillTransformerOptions(EnumProperty* prop, uint32_t mask); - ros::AsyncSpinner spinner_; - ros::CallbackQueue cbqueue_; - D_CloudInfo cloud_infos_; Ogre::SceneNode* scene_node_; diff --git a/src/rviz/default_plugin/point_cloud_display.cpp b/src/rviz/default_plugin/point_cloud_display.cpp index aeaf4e4884..4df23eb9b1 100644 --- a/src/rviz/default_plugin/point_cloud_display.cpp +++ b/src/rviz/default_plugin/point_cloud_display.cpp @@ -50,10 +50,6 @@ PointCloudDisplay::PointCloudDisplay() : point_cloud_common_(new PointCloudCommo " Increasing this is useful if your incoming TF data is delayed significantly " "from your PointCloud data, but it can greatly increase memory usage if the messages are big.", this, SLOT(updateQueueSize())); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue(point_cloud_common_->getCallbackQueue()); } PointCloudDisplay::~PointCloudDisplay() @@ -63,6 +59,9 @@ PointCloudDisplay::~PointCloudDisplay() void PointCloudDisplay::onInitialize() { + // Use the threaded queue for processing of incoming messages + update_nh_.setCallbackQueue(context_->getThreadedQueue()); + MFDClass::onInitialize(); point_cloud_common_->initialize(context_, scene_node_); } diff --git a/src/rviz/default_plugin/relative_humidity_display.cpp b/src/rviz/default_plugin/relative_humidity_display.cpp index ca75fae746..ab40ce2beb 100644 --- a/src/rviz/default_plugin/relative_humidity_display.cpp +++ b/src/rviz/default_plugin/relative_humidity_display.cpp @@ -53,10 +53,6 @@ RelativeHumidityDisplay::RelativeHumidityDisplay() : point_cloud_common_(new Poi "from your RelativeHumidity data, but it can greatly increase memory usage if the " "messages are big.", this, SLOT(updateQueueSize())); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue(point_cloud_common_->getCallbackQueue()); } RelativeHumidityDisplay::~RelativeHumidityDisplay() @@ -66,6 +62,9 @@ RelativeHumidityDisplay::~RelativeHumidityDisplay() void RelativeHumidityDisplay::onInitialize() { + // Use the threaded queue for processing of incoming messages + update_nh_.setCallbackQueue(context_->getThreadedQueue()); + MFDClass::onInitialize(); point_cloud_common_->initialize(context_, scene_node_); diff --git a/src/rviz/default_plugin/temperature_display.cpp b/src/rviz/default_plugin/temperature_display.cpp index c99d72d82a..55bfdb1442 100644 --- a/src/rviz/default_plugin/temperature_display.cpp +++ b/src/rviz/default_plugin/temperature_display.cpp @@ -52,10 +52,6 @@ TemperatureDisplay::TemperatureDisplay() : point_cloud_common_(new PointCloudCom " Increasing this is useful if your incoming TF data is delayed significantly " "from your Temperature data, but it can greatly increase memory usage if the messages are big.", this, SLOT(updateQueueSize())); - - // PointCloudCommon sets up a callback queue with a thread for each - // instance. Use that for processing incoming messages. - update_nh_.setCallbackQueue(point_cloud_common_->getCallbackQueue()); } TemperatureDisplay::~TemperatureDisplay() @@ -65,6 +61,9 @@ TemperatureDisplay::~TemperatureDisplay() void TemperatureDisplay::onInitialize() { + // Use the threaded queue for processing of incoming messages + update_nh_.setCallbackQueue(context_->getThreadedQueue()); + MFDClass::onInitialize(); point_cloud_common_->initialize(context_, scene_node_); diff --git a/src/rviz/displays_panel.cpp b/src/rviz/displays_panel.cpp index 8476481c68..d7ad0c271c 100644 --- a/src/rviz/displays_panel.cpp +++ b/src/rviz/displays_panel.cpp @@ -133,6 +133,7 @@ void DisplaysPanel::onDuplicateDisplay() QList duplicated_displays; QProgressDialog progress_dlg("Duplicating displays...", "Cancel", 0, displays_to_duplicate.size(), this); + vis_manager_->stopUpdate(); progress_dlg.setWindowModality(Qt::WindowModal); progress_dlg.show(); QApplication::processEvents(); // explicitly progress events for update diff --git a/src/rviz/visualization_manager.cpp b/src/rviz/visualization_manager.cpp index f2d6e87575..bc51a285b2 100644 --- a/src/rviz/visualization_manager.cpp +++ b/src/rviz/visualization_manager.cpp @@ -222,6 +222,9 @@ VisualizationManager::VisualizationManager(RenderPanel* render_panel, selection_manager_ = new SelectionManager(this); + update_timer_ = new QTimer; + connect(update_timer_, SIGNAL(timeout()), this, SLOT(onUpdate())); + private_->threaded_queue_threads_.create_thread( boost::bind(&VisualizationManager::threadedQueueThreadFunc, this)); @@ -229,18 +232,16 @@ VisualizationManager::VisualizationManager(RenderPanel* render_panel, ogre_render_queue_clearer_ = new OgreRenderQueueClearer(); Ogre::Root::getSingletonPtr()->addFrameListener(ogre_render_queue_clearer_); - - update_timer_ = new QTimer; - connect(update_timer_, SIGNAL(timeout()), this, SLOT(onUpdate())); } VisualizationManager::~VisualizationManager() { - delete update_timer_; - + update_timer_->stop(); shutting_down_ = true; private_->threaded_queue_threads_.join_all(); + delete update_timer_; + if (selection_manager_) { selection_manager_->setSelection(M_Picked()); @@ -630,9 +631,13 @@ void VisualizationManager::handleChar(QKeyEvent* event, RenderPanel* panel) void VisualizationManager::threadedQueueThreadFunc() { + ros::WallDuration timeout(0.1); while (!shutting_down_) { - private_->threaded_queue_.callOne(ros::WallDuration(0.1)); + if (update_timer_->isActive()) + private_->threaded_queue_.callOne(timeout); + else + timeout.sleep(); } }