diff --git a/src/rviz/default_plugin/effort_display.h b/src/rviz/default_plugin/effort_display.h index 06559d6f1b..a0db3744c8 100644 --- a/src/rviz/default_plugin/effort_display.h +++ b/src/rviz/default_plugin/effort_display.h @@ -536,23 +536,23 @@ class MessageFilterJointStateDisplay: public _RosTopicDisplay virtual void onInitialize() { - // TODO(wjwwood): remove this and use tf2 interface instead + // TODO(wjwwood): remove this and use tf2 interface instead #ifndef _WIN32 # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wdeprecated-declarations" #endif - auto tf_client = context_->getTFClient(); + auto tf_client = context_->getTFClient(); #ifndef _WIN32 # pragma GCC diagnostic pop #endif tf_filter_ = new tf::MessageFilterJointState( *tf_client, - fixed_frame_.toStdString(), 10, update_nh_ ); + fixed_frame_.toStdString(), queue_size_property_->getInt(), update_nh_ ); tf_filter_->connectInput( sub_ ); tf_filter_->registerCallback( boost::bind( &MessageFilterJointStateDisplay::incomingMessage, this, _1 )); - // TODO(wjwwood): remove this and use tf2 interface instead + // TODO(wjwwood): remove this and use tf2 interface instead #ifndef _WIN32 # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wdeprecated-declarations" @@ -587,6 +587,12 @@ class MessageFilterJointStateDisplay: public _RosTopicDisplay context_->queueRender(); } + virtual void updateQueueSize() + { + tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); + subscribe(); + } + virtual void subscribe() { if( !isEnabled() ) @@ -596,7 +602,7 @@ class MessageFilterJointStateDisplay: public _RosTopicDisplay try { - sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 10 ); + sub_.subscribe( update_nh_, topic_property_->getTopicStd(), queue_size_property_->getInt() ); setStatus( StatusProperty::Ok, "Topic", "OK" ); } catch( ros::Exception& e ) diff --git a/src/rviz/default_plugin/fluid_pressure_display.cpp b/src/rviz/default_plugin/fluid_pressure_display.cpp index e403323041..1e61169082 100644 --- a/src/rviz/default_plugin/fluid_pressure_display.cpp +++ b/src/rviz/default_plugin/fluid_pressure_display.cpp @@ -48,12 +48,6 @@ namespace rviz FluidPressureDisplay::FluidPressureDisplay() : point_cloud_common_( new PointCloudCommon( this )) { - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming FluidPressure message queue. " - " 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() ); @@ -76,11 +70,6 @@ void FluidPressureDisplay::onInitialize() subProp("Max Intensity")->setValue(105000); // Typica 'high' atmosphereic pressure in Pascal } -void FluidPressureDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - void FluidPressureDisplay::processMessage( const sensor_msgs::FluidPressureConstPtr& msg ) { // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase diff --git a/src/rviz/default_plugin/fluid_pressure_display.h b/src/rviz/default_plugin/fluid_pressure_display.h index 541aae91ee..1edc897561 100644 --- a/src/rviz/default_plugin/fluid_pressure_display.h +++ b/src/rviz/default_plugin/fluid_pressure_display.h @@ -57,9 +57,6 @@ Q_OBJECT virtual void update( float wall_dt, float ros_dt ); -private Q_SLOTS: - void updateQueueSize(); - protected: /** @brief Do initialization. Overridden from MessageFilterDisplay. */ virtual void onInitialize(); @@ -67,8 +64,6 @@ private Q_SLOTS: /** @brief Process a single message. Overridden from MessageFilterDisplay. */ virtual void processMessage( const sensor_msgs::FluidPressureConstPtr& msg ); - IntProperty* queue_size_property_; - PointCloudCommon* point_cloud_common_; }; diff --git a/src/rviz/default_plugin/illuminance_display.cpp b/src/rviz/default_plugin/illuminance_display.cpp index 6c011a8328..11bef46ae6 100644 --- a/src/rviz/default_plugin/illuminance_display.cpp +++ b/src/rviz/default_plugin/illuminance_display.cpp @@ -48,12 +48,6 @@ namespace rviz IlluminanceDisplay::IlluminanceDisplay() : point_cloud_common_( new PointCloudCommon( this )) { - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming Illuminance message queue. " - " 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() ); @@ -76,11 +70,6 @@ void IlluminanceDisplay::onInitialize() subProp("Max Intensity")->setValue(1000); } -void IlluminanceDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - void IlluminanceDisplay::processMessage( const sensor_msgs::IlluminanceConstPtr& msg ) { // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase diff --git a/src/rviz/default_plugin/illuminance_display.h b/src/rviz/default_plugin/illuminance_display.h index a2efee2c84..0b43787154 100644 --- a/src/rviz/default_plugin/illuminance_display.h +++ b/src/rviz/default_plugin/illuminance_display.h @@ -57,9 +57,6 @@ Q_OBJECT virtual void update( float wall_dt, float ros_dt ); -private Q_SLOTS: - void updateQueueSize(); - protected: /** @brief Do initialization. Overridden from MessageFilterDisplay. */ virtual void onInitialize(); @@ -67,8 +64,6 @@ private Q_SLOTS: /** @brief Process a single message. Overridden from MessageFilterDisplay. */ virtual void processMessage( const sensor_msgs::IlluminanceConstPtr& msg ); - IntProperty* queue_size_property_; - PointCloudCommon* point_cloud_common_; }; diff --git a/src/rviz/default_plugin/laser_scan_display.cpp b/src/rviz/default_plugin/laser_scan_display.cpp index d4c0909df6..91d3157be0 100644 --- a/src/rviz/default_plugin/laser_scan_display.cpp +++ b/src/rviz/default_plugin/laser_scan_display.cpp @@ -50,12 +50,6 @@ LaserScanDisplay::LaserScanDisplay() : point_cloud_common_( new PointCloudCommon( this )) , projector_( new laser_geometry::LaserProjection() ) { - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming LaserScan message queue. " - " 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() ); @@ -73,11 +67,6 @@ void LaserScanDisplay::onInitialize() point_cloud_common_->initialize( context_, scene_node_ ); } -void LaserScanDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - void LaserScanDisplay::processMessage( const sensor_msgs::LaserScanConstPtr& scan ) { sensor_msgs::PointCloudPtr cloud( new sensor_msgs::PointCloud ); diff --git a/src/rviz/default_plugin/laser_scan_display.h b/src/rviz/default_plugin/laser_scan_display.h index 3f8249f2d7..99a836c480 100644 --- a/src/rviz/default_plugin/laser_scan_display.h +++ b/src/rviz/default_plugin/laser_scan_display.h @@ -57,9 +57,6 @@ Q_OBJECT virtual void update( float wall_dt, float ros_dt ); -private Q_SLOTS: - void updateQueueSize(); - protected: /** @brief Do initialization. Overridden from MessageFilterDisplay. */ virtual void onInitialize(); @@ -67,8 +64,6 @@ private Q_SLOTS: /** @brief Process a single message. Overridden from MessageFilterDisplay. */ virtual void processMessage( const sensor_msgs::LaserScanConstPtr& scan ); - IntProperty* queue_size_property_; - PointCloudCommon* point_cloud_common_; laser_geometry::LaserProjection* projector_; diff --git a/src/rviz/default_plugin/point_cloud2_display.cpp b/src/rviz/default_plugin/point_cloud2_display.cpp index 96c90dd11f..4ee82ae06c 100644 --- a/src/rviz/default_plugin/point_cloud2_display.cpp +++ b/src/rviz/default_plugin/point_cloud2_display.cpp @@ -48,12 +48,6 @@ namespace rviz PointCloud2Display::PointCloud2Display() : point_cloud_common_( new PointCloudCommon( this )) { - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming PointCloud2 message queue. " - " 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() ); @@ -70,11 +64,6 @@ void PointCloud2Display::onInitialize() point_cloud_common_->initialize( context_, scene_node_ ); } -void PointCloud2Display::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - void PointCloud2Display::processMessage( const sensor_msgs::PointCloud2ConstPtr& cloud ) { // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase diff --git a/src/rviz/default_plugin/point_cloud2_display.h b/src/rviz/default_plugin/point_cloud2_display.h index 9cb5fab00b..04fba6963a 100644 --- a/src/rviz/default_plugin/point_cloud2_display.h +++ b/src/rviz/default_plugin/point_cloud2_display.h @@ -59,9 +59,6 @@ Q_OBJECT virtual void update( float wall_dt, float ros_dt ); -private Q_SLOTS: - void updateQueueSize(); - protected: /** @brief Do initialization. Overridden from MessageFilterDisplay. */ virtual void onInitialize(); @@ -69,8 +66,6 @@ private Q_SLOTS: /** @brief Process a single message. Overridden from MessageFilterDisplay. */ virtual void processMessage( const sensor_msgs::PointCloud2ConstPtr& cloud ); - IntProperty* queue_size_property_; - PointCloudCommon* point_cloud_common_; }; diff --git a/src/rviz/default_plugin/point_cloud_display.cpp b/src/rviz/default_plugin/point_cloud_display.cpp index f543430b89..bef2f4f36d 100644 --- a/src/rviz/default_plugin/point_cloud_display.cpp +++ b/src/rviz/default_plugin/point_cloud_display.cpp @@ -46,12 +46,6 @@ namespace rviz PointCloudDisplay::PointCloudDisplay() : point_cloud_common_( new PointCloudCommon( this )) { - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming PointCloud message queue. " - " 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() ); @@ -68,11 +62,6 @@ void PointCloudDisplay::onInitialize() point_cloud_common_->initialize( context_, scene_node_ ); } -void PointCloudDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - void PointCloudDisplay::processMessage( const sensor_msgs::PointCloudConstPtr& cloud ) { point_cloud_common_->addMessage( cloud ); diff --git a/src/rviz/default_plugin/point_cloud_display.h b/src/rviz/default_plugin/point_cloud_display.h index 327ba2b7f0..9c2b9ddfc8 100644 --- a/src/rviz/default_plugin/point_cloud_display.h +++ b/src/rviz/default_plugin/point_cloud_display.h @@ -63,9 +63,6 @@ Q_OBJECT virtual void update( float wall_dt, float ros_dt ); -private Q_SLOTS: - void updateQueueSize(); - protected: /** @brief Do initialization. Overridden from MessageFilterDisplay. */ virtual void onInitialize(); @@ -73,8 +70,6 @@ private Q_SLOTS: /** @brief Process a single message. Overridden from MessageFilterDisplay. */ virtual void processMessage( const sensor_msgs::PointCloudConstPtr& cloud ); - IntProperty* queue_size_property_; - PointCloudCommon* point_cloud_common_; }; diff --git a/src/rviz/default_plugin/range_display.cpp b/src/rviz/default_plugin/range_display.cpp index 92551f9fb2..05bf400560 100644 --- a/src/rviz/default_plugin/range_display.cpp +++ b/src/rviz/default_plugin/range_display.cpp @@ -57,10 +57,6 @@ RangeDisplay::RangeDisplay() "Number of prior measurements to display.", this, SLOT( updateBufferLength() )); buffer_length_property_->setMin( 1 ); - - queue_size_property_ = new IntProperty( "Queue Size", 100, - "Size of the tf message filter queue. It usually needs to be set at least as high as the number of sonar frames.", - this, SLOT( updateQueueSize() )); } void RangeDisplay::onInitialize() @@ -84,11 +80,6 @@ void RangeDisplay::reset() updateBufferLength(); } -void RangeDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - void RangeDisplay::updateColorAndAlpha() { Ogre::ColourValue oc = color_property_->getOgreColor(); diff --git a/src/rviz/default_plugin/range_display.h b/src/rviz/default_plugin/range_display.h index 7ec864f847..43c122d49b 100644 --- a/src/rviz/default_plugin/range_display.h +++ b/src/rviz/default_plugin/range_display.h @@ -70,7 +70,6 @@ Q_OBJECT private Q_SLOTS: void updateBufferLength(); void updateColorAndAlpha(); - void updateQueueSize(); private: std::vector cones_; ///< Handles actually drawing the cones @@ -78,7 +77,6 @@ private Q_SLOTS: ColorProperty* color_property_; FloatProperty* alpha_property_; IntProperty* buffer_length_property_; - IntProperty* queue_size_property_; }; } // namespace range_plugin diff --git a/src/rviz/default_plugin/relative_humidity_display.cpp b/src/rviz/default_plugin/relative_humidity_display.cpp index 56105aa609..90660f3aa3 100644 --- a/src/rviz/default_plugin/relative_humidity_display.cpp +++ b/src/rviz/default_plugin/relative_humidity_display.cpp @@ -48,12 +48,6 @@ namespace rviz RelativeHumidityDisplay::RelativeHumidityDisplay() : point_cloud_common_( new PointCloudCommon( this )) { - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming RelativeHumidity message queue. " - " Increasing this is useful if your incoming TF data is delayed significantly " - "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() ); @@ -76,11 +70,6 @@ void RelativeHumidityDisplay::onInitialize() subProp("Max Intensity")->setValue(1.0); // 100% relative humidity } -void RelativeHumidityDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - void RelativeHumidityDisplay::processMessage( const sensor_msgs::RelativeHumidityConstPtr& msg ) { sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2); diff --git a/src/rviz/default_plugin/relative_humidity_display.h b/src/rviz/default_plugin/relative_humidity_display.h index 9eef371e4c..2530a86916 100644 --- a/src/rviz/default_plugin/relative_humidity_display.h +++ b/src/rviz/default_plugin/relative_humidity_display.h @@ -57,9 +57,6 @@ Q_OBJECT virtual void update( float wall_dt, float ros_dt ); -private Q_SLOTS: - void updateQueueSize(); - protected: /** @brief Do initialization. Overridden from MessageFilterDisplay. */ virtual void onInitialize(); @@ -67,8 +64,6 @@ private Q_SLOTS: /** @brief Process a single message. Overridden from MessageFilterDisplay. */ virtual void processMessage( const sensor_msgs::RelativeHumidityConstPtr& msg ); - IntProperty* queue_size_property_; - PointCloudCommon* point_cloud_common_; }; diff --git a/src/rviz/default_plugin/temperature_display.cpp b/src/rviz/default_plugin/temperature_display.cpp index 6d97d0501d..e882c4a9d1 100644 --- a/src/rviz/default_plugin/temperature_display.cpp +++ b/src/rviz/default_plugin/temperature_display.cpp @@ -48,12 +48,6 @@ namespace rviz TemperatureDisplay::TemperatureDisplay() : point_cloud_common_( new PointCloudCommon( this )) { - queue_size_property_ = new IntProperty( "Queue Size", 10, - "Advanced: set the size of the incoming Temperature message queue. " - " 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() ); @@ -77,11 +71,6 @@ void TemperatureDisplay::onInitialize() subProp("Max Intensity")->setValue(100); // Water Boiling } -void TemperatureDisplay::updateQueueSize() -{ - tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); -} - void TemperatureDisplay::processMessage( const sensor_msgs::TemperatureConstPtr& msg ) { sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2); diff --git a/src/rviz/default_plugin/temperature_display.h b/src/rviz/default_plugin/temperature_display.h index 63bf343c11..5e5b573e3a 100644 --- a/src/rviz/default_plugin/temperature_display.h +++ b/src/rviz/default_plugin/temperature_display.h @@ -57,9 +57,6 @@ Q_OBJECT virtual void update( float wall_dt, float ros_dt ); -private Q_SLOTS: - void updateQueueSize(); - protected: /** @brief Do initialization. Overridden from MessageFilterDisplay. */ virtual void onInitialize(); @@ -67,8 +64,6 @@ private Q_SLOTS: /** @brief Process a single message. Overridden from MessageFilterDisplay. */ virtual void processMessage( const sensor_msgs::TemperatureConstPtr& msg ); - IntProperty* queue_size_property_; - PointCloudCommon* point_cloud_common_; }; diff --git a/src/rviz/message_filter_display.h b/src/rviz/message_filter_display.h index 84377bde7c..3bd8fde7dd 100644 --- a/src/rviz/message_filter_display.h +++ b/src/rviz/message_filter_display.h @@ -39,6 +39,7 @@ #include "rviz/display_context.h" #include "rviz/frame_manager.h" +#include "rviz/properties/int_property.h" #include "rviz/properties/ros_topic_property.h" #include "rviz/display.h" @@ -63,14 +64,22 @@ Q_OBJECT "Prefer UDP topic transport", this, SLOT( updateTopic() )); + queue_size_property_ = new IntProperty( "Queue Size", 10, + "Size of TF message filter queue.\n" + "Increasing this is useful if your TF data is delayed significantly " + "w.r.t. your data, but it can greatly increase memory usage as well.", + this, SLOT( updateQueueSize() )); + queue_size_property_->setMin(0); } protected Q_SLOTS: virtual void updateTopic() = 0; + virtual void updateQueueSize() = 0; protected: RosTopicProperty* topic_property_; BoolProperty* unreliable_property_; + IntProperty* queue_size_property_; }; /** @brief Display subclass using a tf2_ros::MessageFilter, templated on the ROS message type. @@ -102,7 +111,7 @@ class MessageFilterDisplay: public _RosTopicDisplay tf_filter_ = new tf2_ros::MessageFilter( *context_->getTF2BufferPtr(), fixed_frame_.toStdString(), - 10, + static_cast(queue_size_property_->getInt()), update_nh_); tf_filter_->connectInput( sub_ ); @@ -141,6 +150,12 @@ class MessageFilterDisplay: public _RosTopicDisplay context_->queueRender(); } + virtual void updateQueueSize() + { + tf_filter_->setQueueSize(static_cast(queue_size_property_->getInt())); + subscribe(); + } + virtual void subscribe() { if( !isEnabled() ) @@ -156,7 +171,8 @@ class MessageFilterDisplay: public _RosTopicDisplay { transport_hint = ros::TransportHints().unreliable(); } - sub_.subscribe( update_nh_, topic_property_->getTopicStd(), 10, transport_hint); + sub_.subscribe( update_nh_, topic_property_->getTopicStd(), + static_cast(queue_size_property_->getInt()), transport_hint); setStatus( StatusProperty::Ok, "Topic", "OK" ); } catch( ros::Exception& e )