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
f690f1de
Unverified
Commit
f690f1de
authored
Apr 30, 2019
by
Maurice Fallon
Committed by
GitHub
Apr 30, 2019
Browse files
Merge pull request #28 from ori-drs/mf-process-from-file
process from file
parents
44dd6119
c36d2dfa
Changes
12
Hide whitespace changes
Inline
Side-by-side
aicp_core/include/aicp_registration/app.hpp
View file @
f690f1de
...
...
@@ -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
0 → 100644
View file @
f690f1de
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
View file @
f690f1de
...
...
@@ -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
View file @
f690f1de
...
...
@@ -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
;
b
re
ak
;
re
turn
;
}
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
View file @
f690f1de
...
...
@@ -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
View file @
f690f1de
...
...
@@ -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
View file @
f690f1de
...
...
@@ -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
;
u
int64_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_
;
u
int64_t
utime_
;
int64_t
utime_
;
ros
::
NodeHandle
&
nh_
;
ros
::
Subscriber
lidar_sub_
;
...
...
aicp_ros/launch/aicp.launch
View file @
f690f1de
...
...
@@ -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
View file @
f690f1de
...
...
@@ -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
View file @
f690f1de
...
...
@@ -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
View file @
f690f1de
...
...
@@ -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
View file @
f690f1de
...
...
@@ -91,7 +91,7 @@ bool VelodyneAccumulatorROS::getFinished() const {
return
finished_
;
}
u
int64_t
VelodyneAccumulatorROS
::
getFinishedTime
()
const
{
int64_t
VelodyneAccumulatorROS
::
getFinishedTime
()
const
{
return
utime_
;
}
...
...
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