Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Gepetto
aicp_mapping
Commits
c36d2dfa
Commit
c36d2dfa
authored
Apr 30, 2019
by
Maurice Fallon
Browse files
make queue a parameter
parent
5ae8f9e9
Changes
4
Hide whitespace changes
Inline
Side-by-side
aicp_core/include/aicp_registration/app.hpp
View file @
c36d2dfa
...
...
@@ -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
View file @
c36d2dfa
...
...
@@ -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
View file @
c36d2dfa
...
...
@@ -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
View file @
c36d2dfa
...
...
@@ -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
();
}
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment