Merge pull request #28 from ori-drs/mf-process-from-file

process from file

aicp_core/include/aicp_registration/app.hpp

@@ -38,7 +38,11 @@ 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;
+    string process_input_clouds_folder;
 };
 
 namespace aicp {
@@ -63,6 +67,10 @@ public:
     ClassificationParams class_params_;
     int online_results_line_;
 
+    void processFromFile(std::string file_path);
+
+    void processCloud(AlignedCloudPtr cloud);
+
     // thread function doing actual work
     void operator()();
 

aicp_core/include/aicp_utils/poseFileReader.hpp

+
+
+class IsometryWithTime
+{
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+public:
+
+    IsometryWithTime(){
+      pose = Eigen::Isometry3d::Identity();
+      sec = 0;
+      nsec = 0;
+      counter = 0;
+    }
+
+    IsometryWithTime(Eigen::Isometry3d pose_in,
+                 int sec_in,
+                 int nsec_in,
+                 int counter_in){
+      pose = pose_in;
+      sec = sec_in;
+      nsec = nsec_in;
+      counter = counter_in;
+    }
+
+    ~IsometryWithTime(){
+    }
+
+    Eigen::Isometry3d pose;
+    int sec;
+    int nsec;
+    int counter;
+};
+
+
+class PoseFileReader
+{
+public:
+
+    PoseFileReader(){
+    }
+
+    ~PoseFileReader(){
+    }
+
+    void readPoseFile(std::string file_name, std::vector< IsometryWithTime > &world_to_body_poses){
+
+        // Read the input poses file:
+        ifstream in( file_name );
+        vector<vector<double>> fields;
+        if (in) {
+            string line;
+            while (getline(in, line)) {
+                if (line.at(0) == '#'){
+                    continue;
+                }
+
+                stringstream sep(line);
+                string field;
+                fields.push_back(vector<double>());
+                while (getline(sep, field, ',')) {
+                    fields.back().push_back(stod(field));
+                }
+            }
+        }
+
+        for (auto row : fields) {
+            Eigen::Isometry3d world_to_body = Eigen::Isometry3d::Identity();
+            world_to_body.translation() << row[3],row[4],row[5];
+            world_to_body.rotate( Eigen::Quaterniond(row[9],row[6],row[7],row[8]) );
+
+            int counter = int(row[0]);
+            int sec = int(row[1]);
+            int nsec = int(row[2]);
+            IsometryWithTime world_to_body_pose = IsometryWithTime(world_to_body, sec, nsec, counter);
+            world_to_body_poses.push_back(world_to_body_pose);
+        }
+    }
+
+};
\ No newline at end of file

aicp_core/include/aicp_utils/timing.hpp

@@ -11,7 +11,7 @@ class TimingUtils{
     ~TimingUtils(){}   
 
     static void tic();
-    static void toc();
+    static void toc(std::string message = "");
     static std::string currentDateTime();
 
     static void sleepSeconds(clock_t sec);

aicp_core/src/registration/app.cpp

@@ -5,6 +5,7 @@
 
 #include "aicp_utils/timing.hpp"
 #include "aicp_utils/common.hpp"
+#include "aicp_utils/poseFileReader.hpp"
 
 namespace aicp {
 
@@ -224,7 +225,9 @@ void App::runAicpPipeline(pcl::PointCloud<pcl::PointXYZ>::Ptr& reference_prefilt
     =          Overlap         =
     ===========================*/
 
+    TimingUtils::tic();
     computeOverlap(reference_prefiltered, reading_prefiltered, reference_pose, reading_pose);
+    TimingUtils::toc("computeOverlap");
 
     /*=================================
     =          Alignment Risk         =
@@ -233,33 +236,51 @@ void App::runAicpPipeline(pcl::PointCloud<pcl::PointXYZ>::Ptr& reference_prefilt
     if (cl_cfg_.failure_prediction_mode)
         computeAlignmentRisk(reference_prefiltered, reading_prefiltered, reference_pose, reading_pose);
 
+    TimingUtils::tic();
     /*================================
     =          Registration          =
     ================================*/
     if(!cl_cfg_.failure_prediction_mode ||                      // if alignment risk disabled
        risk_prediction_(0,0) <= class_params_.svm.threshold)    // or below threshold
         computeRegistration(*reference_prefiltered, *reading_prefiltered, T);
+    TimingUtils::toc("computeRegistration");
 }
 
-void App::operator()() {
-    running_ = true;
-    while (running_) {
-        std::unique_lock<std::mutex> lock(worker_mutex_);
-        // Wait for notification from planarLidarHandler
-        worker_condition_.wait_for(lock, std::chrono::milliseconds(1000));
 
-        // Copy current workload from cloud queue to work queue
-        std::list<AlignedCloudPtr> work_queue;
-        {
-            std::unique_lock<std::mutex> lock(data_mutex_);
-            while (!cloud_queue_.empty()) {
-                work_queue.push_back(cloud_queue_.front());
-                cloud_queue_.pop_front();
-            }
+void App::processFromFile(std::string file_path){
+    std::cout << "starting processFromFile\n";
+
+    // Read all poses csv file:
+    std::stringstream ss;
+    ss << file_path << "/aicp_input_poses.csv";
+    std::vector< IsometryWithTime > world_to_body_poses;
+    PoseFileReader reader;
+    reader.readPoseFile(ss.str(), world_to_body_poses);
+
+    // Process each cloud successively
+    for (auto pose_with_time : world_to_body_poses) {
+        std::stringstream ss2;
+        ss2 << file_path << "/cloud_" << pose_with_time.counter << "_" << pose_with_time.sec << "_" << pose_with_time.nsec << ".pcd";
+        std::cout << ss2.str() << "\n";
+
+        int64_t utime = pose_with_time.sec*1E6 + pose_with_time.nsec;
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr accumulated_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+        if (pcl::io::loadPCDFile<pcl::PointXYZ> (ss2.str(), *accumulated_cloud) == -1){
+           std::cout << "Couldn't read file " << ss2.str() << "\n";
+           return;
         }
 
-        // Process workload
-        for (auto cloud : work_queue) {
+        AlignedCloudPtr current_cloud (new AlignedCloud(utime,
+                                                        accumulated_cloud,
+                                                        pose_with_time.pose));
+        processCloud(current_cloud);
+    }
+}
+
+
+void App::processCloud(AlignedCloudPtr cloud){
+
 
             // First point cloud (becomes first reference)
             if(!cl_cfg_.localize_against_prior_map &&
@@ -294,8 +315,9 @@ void App::operator()() {
                 first_cloud_initialized_ = true;
             }
             else {
-                TimingUtils::tic();
 
+                TimingUtils::tic();
+                TimingUtils::tic();
                 if (cl_cfg_.verbose)
                 {
                     // Publish original reading cloud
@@ -326,6 +348,7 @@ void App::operator()() {
                     filtered_read << "/reading_prefiltered.pcd";
                     pcd_writer_.write<pcl::PointXYZ> (filtered_read.str (), *read_prefiltered, false);
                 }
+                TimingUtils::toc("setAndFilterReading");
 
                 /*=====================================
                 =          AICP Registration          =
@@ -335,6 +358,7 @@ void App::operator()() {
 
                 runAicpPipeline(ref_prefiltered, read_prefiltered, ref_pose, read_pose, correction);
 
+                TimingUtils::tic();
                 if(!cl_cfg_.failure_prediction_mode ||                      // if alignment risk disabled
                    risk_prediction_(0,0) <= class_params_.svm.threshold)    // or below threshold
                 {
@@ -345,7 +369,7 @@ void App::operator()() {
                          aligned_clouds_graph_->getNbClouds() != 0)
                     {
                         cout << "[Main] -----> WRONG ALIGNMENT: DROPPED POINT CLOUD" << endl;
-                        break;
+                        return;
                     }
 
                     pcl::transformPointCloud (*read_prefiltered, *output, correction);
@@ -385,12 +409,14 @@ void App::operator()() {
                     updates_counter_ ++;
                     cout << "[Main] -----> ALIGNMENT RISK REFERENCE UPDATE" << endl;
                 }
+                TimingUtils::toc("updateReference");
 
                 initialT_ = correction * initialT_;
 
                 /*======================================
                 =          Save and Visualize          =
                 ======================================*/
+                TimingUtils::tic();
 
                 // Store chain of corrections for publishing
                 total_correction_ = fromMatrix4fToIsometry3d(initialT_);
@@ -477,8 +503,8 @@ void App::operator()() {
                     aligned_read << "/reading_aligned.pcd";
                     pcd_writer_.write<pcl::PointXYZ> (aligned_read.str (), *aligned_clouds_graph_->getLastCloud()->getCloud(), false);
                 }
-
-                TimingUtils::toc();
+                TimingUtils::toc("postProcessing");
+                TimingUtils::toc("fullLoop");
 
                 cout << "============================" << endl
                      << "[Main] Summary:" << endl
@@ -493,6 +519,32 @@ void App::operator()() {
                 cout << "Updates: " << updates_counter_ << endl;
             }
             cout << "--------------------------------------------------------------------------------------" << endl;
+
+
+}
+
+
+
+void App::operator()() {
+    running_ = true;
+    while (running_) {
+        std::unique_lock<std::mutex> lock(worker_mutex_);
+        // Wait for notification from planarLidarHandler
+        worker_condition_.wait_for(lock, std::chrono::milliseconds(1000));
+
+        // Copy current workload from cloud queue to work queue
+        std::list<AlignedCloudPtr> work_queue;
+        {
+            std::unique_lock<std::mutex> lock(data_mutex_);
+            while (!cloud_queue_.empty()) {
+                work_queue.push_back(cloud_queue_.front());
+                cloud_queue_.pop_front();
+            }
+        }
+
+        // Process workload
+        for (auto cloud : work_queue) {
+            processCloud(cloud);
         }
     }
 }

aicp_core/src/utils/timing.cpp

@@ -7,10 +7,10 @@ void TimingUtils::tic() {
   tictoc_stack.push(clock());   
 }
 
-void TimingUtils::toc() {
+void TimingUtils::toc(std::string message) {
   std::cout << "Time elapsed: "
   << ((double)(clock() - tictoc_stack.top())) / CLOCKS_PER_SEC
-  << " sec"
+  << " sec " << message
   << std::endl;
   tictoc_stack.pop();
 }

aicp_ros/include/aicp_ros/app_ros.hpp

@@ -24,11 +24,17 @@ public:
            const ClassificationParams& class_params);
 
     inline ~AppROS() {
+        if (input_poses_file_.is_open()) {
+            input_poses_file_.close();
+        }
+
         delete accu_;
         delete vis_ros_;
         delete talk_ros_;
     }
 
+    void writeCloudToFile(AlignedCloudPtr cloud);
+
     // Subscriber callabacks
     void velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr& laser_msg_in);
     void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg_in);
@@ -45,6 +51,9 @@ public:
 private:
     ros::NodeHandle& nh_;
 
+    std::ofstream input_poses_file_;
+    int input_clouds_counter_;
+
     Eigen::Isometry3d world_to_body_;
     Eigen::Isometry3d world_to_body_previous_;
     tf::Pose temp_tf_pose_;

aicp_ros/include/aicp_ros/velodyne_accumulator.hpp

@@ -17,7 +17,7 @@ struct VelodyneAccumulatorConfig
     int batch_size = 10;
     double max_range = 30;
     double min_range = 0.5;
-    std::string lidar_topic = "/velodyne/point_cloud_filtered";
+    std::string lidar_topic = "/point_cloud_filter/velodyne/point_cloud_filtered";
     std::string inertial_frame = "/odom";
 };
 
@@ -35,7 +35,7 @@ public:
 
     uint16_t getCounter() const;
     bool getFinished() const;
-    uint64_t getFinishedTime() const;
+    int64_t getFinishedTime() const;
 
     const PointCloud& getCloud();
     void clearCloud();
@@ -53,7 +53,7 @@ private:
     // implicitly discarding intensities from the clouds
     PointCloud point_cloud_;
     PointCloud accumulated_point_cloud_;
-    uint64_t utime_;
+    int64_t utime_;
 
     ros::NodeHandle& nh_;
     ros::Subscriber lidar_sub_;

aicp_ros/launch/aicp.launch

@@ -20,6 +20,11 @@
   <arg name="saveProbs"                        default="$(find aicp_core)/data/classification/probs_opencv3.txt"/>
   <arg name="modelLocation"                    default="$(find aicp_core)/data/models/svm_opencv3.xml" />
 
+
+  <arg name="write_input_clouds_to_file"       default="false"/>
+  <arg name="process_input_clouds_from_file"   default="false"/>
+  <arg name="process_input_clouds_folder"      default="/tmp/aicp_data/"/>
+
   <!-- Run AICP (command line configuration) -->
   <node pkg="aicp_ros" type="aicp_ros_node" name="aicp_ros_node" output="screen">
 
@@ -56,11 +61,21 @@
     <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 -->
     <param name="verbose"                       value="false" />
 
+    <!-- Write out incoming data to file -->
+    <param name="write_input_clouds_to_file"    value="$(arg write_input_clouds_to_file)" />
+    <!-- Read incoming data from file -->
+    <param name="process_input_clouds_from_file" value="$(arg process_input_clouds_from_file)" />
+    <!-- Read incoming data from file -->
+    <param name="process_input_clouds_folder"   value="$(arg process_input_clouds_folder)" />
+
   </node>
 
 </launch>

aicp_ros/launch/aicp_mapping.launch

@@ -8,10 +8,19 @@
   <arg name="localize_against_prior_map"   default="false"/> <!-- ! should always be false in this launch file ! -->
   <arg name="localize_against_built_map"   default="false"/> <!-- implicit loop closure when revisit known place -->
 
+  <!-- options for post processing -->
+  <arg name="write_input_clouds_to_file"       default="false"/>
+  <arg name="process_input_clouds_from_file"   default="false"/>
+  <arg name="process_input_clouds_folder"      default="/tmp/aicp_data"/>
+
   <include file="$(find aicp_ros)/launch/aicp.launch">
     <arg name="load_map_from_file"         value="$(arg load_map_from_file)"/>
     <arg name="localize_against_prior_map" value="$(arg localize_against_prior_map)"/>
     <arg name="localize_against_built_map" value="$(arg localize_against_built_map)"/>
+
+    <arg name="write_input_clouds_to_file"     value="$(arg write_input_clouds_to_file)"/>
+    <arg name="process_input_clouds_from_file" value="$(arg process_input_clouds_from_file)"/>
+    <arg name="process_input_clouds_folder"    value="$(arg process_input_clouds_folder)"/>
   </include>
   
   <group if="$(arg load_map_from_file)">

aicp_ros/src/aicp_ros_node.cpp

@@ -27,16 +27,20 @@ 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...
     cl_cfg.verbose = false; // enable visualization for debug
+    cl_cfg.write_input_clouds_to_file = false; // write the raw incoming point clouds to a folder, for post processing
+    cl_cfg.process_input_clouds_from_file = false;  // process raw incoming point cloud from a folder
+    cl_cfg.process_input_clouds_folder = "/tmp/aicp_data";
 
     aicp::VelodyneAccumulatorConfig va_cfg;
     va_cfg.batch_size = 80; // 240 is about 1 sweep at 5RPM // 80 is about 1 sweep at 15RPM
     va_cfg.min_range = 0.50; // 1.85; // remove all the short range points
     va_cfg.max_range = 15.0; // we can set up to 30 meters (guaranteed range)
-    va_cfg.lidar_topic ="/velodyne/point_cloud_filtered";
+    va_cfg.lidar_topic ="/point_cloud_filter/velodyne/point_cloud_filtered";
     va_cfg.inertial_frame = "/odom";
 
     nh.getParam("registration_config_file", cl_cfg.registration_config_file);
@@ -53,10 +57,15 @@ 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);
     nh.getParam("verbose", cl_cfg.verbose);
+    nh.getParam("write_input_clouds_to_file", cl_cfg.write_input_clouds_to_file);
+    nh.getParam("process_input_clouds_from_file", cl_cfg.process_input_clouds_from_file);
+    nh.getParam("process_input_clouds_folder", cl_cfg.process_input_clouds_folder);
+
 
     nh.getParam("batch_size", va_cfg.batch_size);
     nh.getParam("min_range", va_cfg.min_range);
@@ -100,19 +109,25 @@ int main(int argc, char** argv){
                                                        overlap_params,
                                                        classification_params));
 
-    // Subscribers
-    ros::Subscriber lidar_sub = nh.subscribe(va_cfg.lidar_topic, 100, &aicp::AppROS::velodyneCallBack, app.get());
-    ros::Subscriber pose_sub = nh.subscribe(cl_cfg.pose_body_channel, 100, &aicp::AppROS::robotPoseCallBack, app.get());
-    ros::Subscriber marker_sub = nh.subscribe("/interaction_marker/pose", 100, &aicp::AppROS::interactionMarkerCallBack, app.get());
+    if (!cl_cfg.process_input_clouds_from_file){
+
+        // Subscribers
+        ros::Subscriber lidar_sub = nh.subscribe(va_cfg.lidar_topic, 100, &aicp::AppROS::velodyneCallBack, app.get());
+        ros::Subscriber pose_sub = nh.subscribe(cl_cfg.pose_body_channel, 100, &aicp::AppROS::robotPoseCallBack, app.get());
+        ros::Subscriber marker_sub = nh.subscribe("/interaction_marker/pose", 100, &aicp::AppROS::interactionMarkerCallBack, app.get());
 
-    // Advertise services (using service published by anybotics icp_tools ui)
-    ros::ServiceServer load_map_server_ = nh.advertiseService("/icp_tools/load_map_from_file", &aicp::AppROS::loadMapFromFileCallBack, app.get());
-    ros::ServiceServer go_back_server_ = nh.advertiseService("/aicp/go_back_request", &aicp::AppROS::goBackRequestCallBack, app.get());
+        // Advertise services (using service published by anybotics icp_tools ui)
+        ros::ServiceServer load_map_server_ = nh.advertiseService("/icp_tools/load_map_from_file", &aicp::AppROS::loadMapFromFileCallBack, app.get());
+        ros::ServiceServer go_back_server_ = nh.advertiseService("/aicp/go_back_request", &aicp::AppROS::goBackRequestCallBack, app.get());
 
-    ROS_INFO_STREAM("[Aicp] Waiting for input messages...");
+        ROS_INFO_STREAM("[Aicp] Waiting for input messages...");
 
-    app->run();
-    ros::spin();
+        app->run();
+        ros::spin();
+
+    }else{
+        app->processFromFile(cl_cfg.process_input_clouds_folder);
+    }
 
     return 0;
 }

aicp_ros/src/app_ros.cpp

@@ -49,6 +49,20 @@ AppROS::AppROS(ros::NodeHandle &nh,
         alignability_pub_ = nh_.advertise<std_msgs::Float32>("/aicp/alignability",10);
         risk_pub_ = nh_.advertise<std_msgs::Float32>("/aicp/alignment_risk",10);
     }
+
+    // Write the incoming data to file. This should be false when running live
+    if (cl_cfg_.write_input_clouds_to_file)
+    {
+        ROS_WARN_STREAM("[Aicp] Writing input clouds to file. Only do this in post processing");
+        std::stringstream input_poses_filename;
+        input_poses_filename << data_directory_path_.str() << "/aicp_input_poses.csv";
+
+        input_poses_file_.open (input_poses_filename.str().c_str() );
+        input_poses_file_ << "# counter, sec, nsec, x, y, z, qx, qy, qz, qw\n";
+        input_poses_file_.flush();
+        input_clouds_counter_ = 0;
+    }
+
 }
 
 void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose_msg_in)
@@ -134,6 +148,33 @@ void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedCon
     pose_initialized_ = true;
 }
 
+
+void AppROS::writeCloudToFile(AlignedCloudPtr cloud){
+    // Extract the data from AlignedCloud
+    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud = cloud->getCloud();
+    Eigen::Isometry3d pose = cloud->getPriorPose();
+    Eigen::Quaterniond quat(pose.rotation());
+    int64_t utime = cloud->getUtime();
+    int64_t sec = floor(utime * 1E-6);
+    int64_t nsec = utime - sec* 1E6;
+
+    // Write the base-to-odom estimate
+    std::stringstream ss;
+    ss << input_clouds_counter_ << ", " << sec << ", " << nsec << ", ";
+    ss << pose.translation().x() << ", " << pose.translation().y() << ", " << pose.translation().z() << ", ";
+    ss << quat.x() << ", " << quat.y() << ", " << quat.z() << ", " << quat.w();
+    input_poses_file_ << ss.str() << "\n";
+    input_poses_file_.flush();
+
+    // Write the point cloud
+    std::stringstream ss2;
+    ss2 << data_directory_path_.str() << "/cloud_" << input_clouds_counter_ << "_" << sec << "_" << nsec << ".pcd";
+    pcd_writer_.write<pcl::PointXYZ> (ss2.str (), *point_cloud, true);
+
+    input_clouds_counter_++;
+}
+
+
 void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_msg_in){
     if (!pose_initialized_){
         ROS_WARN_STREAM("[Aicp] Pose not initialized, waiting for pose prior...");
@@ -180,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_);
 
@@ -190,16 +230,20 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
                                                                 world_to_body_));
                 world_to_body_previous_ = world_to_body_;
 
+                if (cl_cfg_.write_input_clouds_to_file)
+                    writeCloudToFile(current_cloud);
+
                 // Stack current cloud into queue
                 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();
                 }
             }

aicp_ros/src/velodyne_accumulator.cpp

@@ -91,7 +91,7 @@ bool VelodyneAccumulatorROS::getFinished() const {
     return finished_;
 }
 
-uint64_t VelodyneAccumulatorROS::getFinishedTime() const{
+int64_t VelodyneAccumulatorROS::getFinishedTime() const{
     return utime_;
 }