make queue a parameter

aicp_core/include/aicp_registration/app.hpp

@@ -38,6 +38,7 @@ struct CommandLineConfig
     bool failure_prediction_mode;
     int reference_update_frequency;
     float max_correction_magnitude;
+    int max_queue_size;
     bool verbose;
     bool write_input_clouds_to_file;
     bool process_input_clouds_from_file;

aicp_ros/launch/aicp.launch

@@ -61,6 +61,9 @@
     <param name="reference_update_frequency"    value="5" />
     <!-- Max allowed correction magnitude (probably failed alignment otherwise) -->
     <param name="max_correction_magnitude"      value="1.0" /> <!-- 1.5 to generate Ground Truth (David IROS19) -->
+    <!-- Max length of the queue of accumulated point clouds. was 100 previously -->
+    <param name="max_queue_size"      value="1" />
+
     <!-- 3D point cloud characteristics -->
     <param name="batch_size"                    value="7" />
   	<!-- Visualize and store -->

aicp_ros/src/aicp_ros_node.cpp

@@ -27,6 +27,7 @@ int main(int argc, char** argv){
     cl_cfg.reference_update_frequency = 5;
     cl_cfg.max_correction_magnitude = 0.5; // Max allowed correction magnitude
                                            // (probably failed alignment otherwise)
+    cl_cfg.max_queue_size = 3; // maximum length of the queue of accumulated point clouds. was 100 previously
 
     cl_cfg.pose_body_channel = "/state_estimator/pose_in_odom";
     cl_cfg.output_channel = "/aicp/pose_corrected"; // Create new channel...
@@ -56,6 +57,7 @@ int main(int argc, char** argv){
     nh.getParam("failure_prediction_mode", cl_cfg.failure_prediction_mode);
     nh.getParam("reference_update_frequency", cl_cfg.reference_update_frequency);
     nh.getParam("max_correction_magnitude", cl_cfg.max_correction_magnitude);
+    nh.getParam("max_queue_size", cl_cfg.max_queue_size);
 
     nh.getParam("pose_body_channel", cl_cfg.pose_body_channel);
     nh.getParam("output_channel", cl_cfg.output_channel);

aicp_ros/src/app_ros.cpp

@@ -221,7 +221,6 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
 //            vis_->publishCloud(accumulated_cloud, 10, "/aicp/accumulated_cloud", accu_->getFinishedTime());
 
             // Push this cloud onto the work queue (mutex safe)
-            const int max_queue_size = 3;
             {
                 std::unique_lock<std::mutex> lock(data_mutex_);
 
@@ -238,13 +237,13 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
                 cloud_queue_.push_back(current_cloud);
                 //cout << "[App ROS] cloud_queue_ size: " << cloud_queue_.size() << endl;
 
-                if (cloud_queue_.size() > max_queue_size) {
+                if (cloud_queue_.size() > cl_cfg_.max_queue_size) {
                     cout << "|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n";
                     cout << "|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n";
                     cout << "[App ROS] WARNING: dropping " <<
-                            (cloud_queue_.size()-max_queue_size) << " clouds." << endl;
+                            (cloud_queue_.size()-cl_cfg_.max_queue_size) << " clouds." << endl;
                 }
-                while (cloud_queue_.size() > max_queue_size) {
+                while (cloud_queue_.size() > cl_cfg_.max_queue_size) {
                     cloud_queue_.pop_front();
                 }
             }