Commit c36d2dfa authored by Maurice Fallon's avatar Maurice Fallon
Browse files

make queue a parameter

parent 5ae8f9e9
......@@ -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;
......
......@@ -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 -->
......
......@@ -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);
......
......@@ -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();
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment