Merge pull request #2724 from nakane11/virtual-camera

[jsk_perception/VirtualCameraMono] Add queue_size param

doc/jsk_perception/nodes/virtual_camera_mono.md

@@ -51,6 +51,11 @@ Calculate perspective transformation from TF frame and apply it to the input ima
 
   Frame ID of virtual camera used in published topics.
 
+* `~queue_size` (Int, default: `1`)
+
+  How many messages you allow about the subscriber to keep in the queue.
+  This should be big when there is much difference about delay between two topics.
+
 * `~initial_pos` (List of Float, default: `[0.7, 0.0, 0.0]`)
 
   Initial position of virtual camera relative to `~frame_id`.

jsk_perception/include/jsk_perception/virtual_camera_mono.h

@@ -53,6 +53,7 @@ namespace jsk_perception
     tf::StampedTransform trans_; // transform to virtual camera
     geometry_msgs::PolygonStamped poly_; // target polygon to transform image
     int interpolation_method_;
+    int queue_size_;
 
   private:
 

jsk_perception/src/virtual_camera_mono.cpp

@@ -20,7 +20,7 @@ namespace jsk_perception
     pnh_->param("frame_id", trans_.frame_id_, std::string("/elevator_inside_panel"));
     pnh_->param("child_frame_id", trans_.child_frame_id_, std::string("/virtual_camera_frame"));
     ROS_INFO("VirutalCmaeraMono(%s) frame_id: %s, chid_frame_id: %s", ros::this_node::getName().c_str(), trans_.frame_id_.c_str(), trans_.child_frame_id_.c_str());
-
+    pnh_->param("queue_size", queue_size_, 1);
     std::vector<double> initial_pos, initial_rot;
     if (jsk_topic_tools::readVectorParameter(*pnh_, "initial_pos", initial_pos)) {
       trans_.setOrigin(tf::Vector3(initial_pos[0], initial_pos[1], initial_pos[2]));
@@ -80,7 +80,7 @@ namespace jsk_perception
   {
     ROS_INFO("Subscribing to image topic");
     it_.reset(new image_transport::ImageTransport(*nh_));
-    sub_ = it_->subscribeCamera("image", 1, &VirtualCameraMono::imageCb, this);
+    sub_ = it_->subscribeCamera("image", queue_size_, &VirtualCameraMono::imageCb, this);
     ros::V_string names = boost::assign::list_of("image");
     jsk_topic_tools::warnNoRemap(names);
   }