Exemplo n.º 1
0
ImageDisplayBase::ImageDisplayBase() :
    Display()
    , it_(update_nh_)
    , sub_()
    , tf_filter_()
    , messages_received_(0)
{
  topic_property_ = new RosTopicProperty("Image Topic", "",
                                         QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
                                         "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");

  scanForTransportSubscriberPlugins();

}
DepthCloudDisplay::DepthCloudDisplay()
  : rviz::Display()
  , messages_received_(0)
  , depthmap_it_(update_nh_)
  , depthmap_sub_()
  , rgb_it_ (update_nh_)
  , rgb_sub_()
  , cameraInfo_sub_()
  , queue_size_(5)
{

  // Depth map properties
  depth_topic_property_ = new RosTopicProperty("Depth Map Topic", "",
                                         QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
                                         "sensor_msgs::Image topic to subscribe to.", this, SLOT( updateTopic() ));

  depth_transport_property_ = new EnumProperty("Depth Map Transport Hint", "raw", "Preferred method of sending images.", this,
                                         SLOT( updateTopic() ));

  connect(depth_transport_property_, SIGNAL( requestOptions( EnumProperty* )), this,
          SLOT( fillTransportOptionList( EnumProperty* )));

  depth_transport_property_->setStdString("raw");

  // RGB image properties
  rgb_topic_property_ = new RosTopicProperty("RGB Image Topic", "",
                                         QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
                                         "sensor_msgs::Image topic to subscribe to.", this, SLOT( updateTopic() ));

  rgb_transport_property_ = new EnumProperty("RGB Transport Hint", "raw", "Preferred method of sending images.", this,
                                         SLOT( updateTopic() ));


  connect(rgb_transport_property_, SIGNAL( requestOptions( EnumProperty* )), this,
          SLOT( fillTransportOptionList( EnumProperty* )));

  rgb_transport_property_->setStdString("raw");


  // Queue size property
  queue_size_property_ = new IntProperty( "Queue Size", queue_size_,
                                          "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 );


  // Instantiate PointCloudCommon class for displaying point clouds
  pointcloud_common_ = new PointCloudCommon(this);

  // PointCloudCommon sets up a callback queue with a thread for each
  // instance.  Use that for processing incoming messages.
  update_nh_.setCallbackQueue( pointcloud_common_->getCallbackQueue() );

  // Scan for available transport plugins
  scanForTransportSubscriberPlugins();

}
void StereoImageDisplayBase::onInitialize()
{
  it_.reset( new image_transport::ImageTransport( update_nh_ ));
  scanForTransportSubscriberPlugins();
}