Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
dd765961
A
apollo
项目概览
Pinoxchio
/
apollo
与 Fork 源项目一致
从无法访问的项目Fork
通知
2
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
0
列表
看板
标记
里程碑
合并请求
0
Wiki
0
Wiki
分析
仓库
DevOps
项目成员
Pages
A
apollo
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
0
Issue
0
列表
看板
标记
里程碑
合并请求
0
合并请求
0
Pages
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
提交
Issue看板
体验新版 GitCode,发现更多精彩内容 >>
提交
dd765961
编写于
6月 27, 2019
作者:
a_big_pig
提交者:
Runxin He
6月 27, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
first ten files fixed
上级
214762f5
变更
10
隐藏空白更改
内联
并排
Showing
10 changed file
with
1635 addition
and
0 deletion
+1635
-0
modules/map/tools/map_datachecker/server/alignment.h
modules/map/tools/map_datachecker/server/alignment.h
+141
-0
modules/map/tools/map_datachecker/server/alignment_agent.h
modules/map/tools/map_datachecker/server/alignment_agent.h
+186
-0
modules/map/tools/map_datachecker/server/channel_verify.cc
modules/map/tools/map_datachecker/server/channel_verify.cc
+220
-0
modules/map/tools/map_datachecker/server/channel_verify.h
modules/map/tools/map_datachecker/server/channel_verify.h
+84
-0
modules/map/tools/map_datachecker/server/channel_verify_agent.cc
.../map/tools/map_datachecker/server/channel_verify_agent.cc
+229
-0
modules/map/tools/map_datachecker/server/channel_verify_agent.h
...s/map/tools/map_datachecker/server/channel_verify_agent.h
+73
-0
modules/map/tools/map_datachecker/server/eight_route.cc
modules/map/tools/map_datachecker/server/eight_route.cc
+165
-0
modules/map/tools/map_datachecker/server/eight_route.h
modules/map/tools/map_datachecker/server/eight_route.h
+48
-0
modules/map/tools/map_datachecker/server/laps_checker.cc
modules/map/tools/map_datachecker/server/laps_checker.cc
+410
-0
modules/map/tools/map_datachecker/server/laps_checker.h
modules/map/tools/map_datachecker/server/laps_checker.h
+79
-0
未找到文件。
modules/map/tools/map_datachecker/server/alignment.h
0 → 100644
浏览文件 @
dd765961
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once
#include <memory>
#include <vector>
#include "modules/map/tools/map_datachecker/proto/collection_error_code.pb.h"
#include "modules/map/tools/map_datachecker/server/common.h"
namespace
apollo
{
namespace
hdmap
{
typedef
struct
BadOrGoodPoseInfo
{
BadOrGoodPoseInfo
()
:
start_time
(
-
1.0
),
end_time
(
-
1.0
),
pose_count
(
0
)
{}
double
start_time
;
double
end_time
;
int
pose_count
;
}
BadOrGoodPoseInfo
;
class
Alignment
{
public:
explicit
Alignment
(
std
::
shared_ptr
<
JSonConf
>
sp_conf
)
:
return_state_
(
ErrorCode
::
SUCCESS
),
sp_conf_
(
sp_conf
),
sp_good_pose_info_
(
std
::
make_shared
<
BadOrGoodPoseInfo
>
()),
sp_bad_pose_info_
(
std
::
make_shared
<
BadOrGoodPoseInfo
>
())
{}
virtual
~
Alignment
()
{}
virtual
ErrorCode
Process
(
const
std
::
vector
<
FramePose
>&
poses
)
=
0
;
virtual
void
Reset
()
=
0
;
virtual
double
GetProgress
()
const
{
return
progress_
;
}
virtual
void
SetStartTime
(
double
start_time
)
{
start_time_
=
start_time
;
}
virtual
void
SetEndTime
(
double
end_time
)
{
end_time_
=
end_time
;
}
virtual
void
UpdateBadPoseInfo
(
const
FramePose
&
pose
)
{
UpdatePoseInfo
(
pose
,
sp_bad_pose_info_
);
}
virtual
void
ClearBadPoseInfo
()
{
ClearPoseInfo
(
sp_bad_pose_info_
);
}
virtual
void
UpdateGoodPoseInfo
(
const
FramePose
&
pose
)
{
UpdatePoseInfo
(
pose
,
sp_good_pose_info_
);
}
virtual
void
ClearGoodPoseInfo
()
{
ClearPoseInfo
(
sp_good_pose_info_
);
}
virtual
bool
IsGoodPose
(
const
std
::
vector
<
FramePose
>&
poses
,
int
pose_index
)
{
if
(
pose_index
<=
0
||
pose_index
>=
static_cast
<
int
>
(
poses
.
size
()))
{
AINFO
<<
"params error. poses size:"
<<
poses
.
size
()
<<
",pose_index:"
<<
pose_index
;
return
false
;
}
unsigned
int
position_type
=
poses
[
pose_index
].
position_type
;
float
diff_age
=
poses
[
pose_index
].
diff_age
;
double
local_std
=
poses
[
pose_index
].
local_std
;
if
(
sp_conf_
->
position_type_range
.
find
(
position_type
)
!=
sp_conf_
->
position_type_range
.
end
()
&&
diff_age
>=
sp_conf_
->
diff_age_range
.
first
&&
diff_age
<=
sp_conf_
->
diff_age_range
.
second
&&
local_std
<=
sp_conf_
->
local_std_upper_limit
)
{
return
true
;
}
return
false
;
}
ErrorCode
GetReturnState
()
const
{
return
return_state_
;
}
protected:
void
UpdatePoseInfo
(
const
FramePose
&
pose
,
std
::
shared_ptr
<
BadOrGoodPoseInfo
>
sp_pose_info
)
{
BadOrGoodPoseInfo
&
pose_info
=
*
sp_pose_info
;
if
(
pose_info
.
pose_count
==
0
)
{
pose_info
.
start_time
=
pose
.
time_stamp
;
++
pose_info
.
pose_count
;
AINFO
<<
"update start time: "
<<
std
::
to_string
(
pose_info
.
start_time
)
<<
",pose count: "
<<
pose_info
.
pose_count
;
}
else
{
pose_info
.
end_time
=
pose
.
time_stamp
;
++
pose_info
.
pose_count
;
AINFO
<<
"update start time: "
<<
std
::
to_string
(
pose_info
.
start_time
)
<<
",pose count: "
<<
pose_info
.
pose_count
;
}
}
void
ClearPoseInfo
(
std
::
shared_ptr
<
BadOrGoodPoseInfo
>
sp_pose_info
)
{
BadOrGoodPoseInfo
&
pose_info
=
*
sp_pose_info
;
pose_info
.
start_time
=
-
1.0
;
pose_info
.
end_time
=
-
1.0
;
pose_info
.
pose_count
=
0
;
}
int
TimeToIndex
(
const
std
::
vector
<
FramePose
>&
poses
,
double
time
)
{
size_t
size
=
poses
.
size
();
if
(
size
==
0
||
time
<=
0
)
{
return
-
1
;
}
for
(
size_t
i
=
0
;
i
<
size
;
++
i
)
{
if
(
poses
[
i
].
time_stamp
>=
time
)
{
return
static_cast
<
int
>
(
i
);
}
}
return
static_cast
<
int
>
(
size
);
}
protected:
double
progress_
;
double
last_progress_
;
double
start_time_
;
double
end_time_
;
int
start_index_
;
double
end_index_
;
ErrorCode
return_state_
;
std
::
shared_ptr
<
JSonConf
>
sp_conf_
=
nullptr
;
// BadOrGoodPoseInfo _bad_pose_info, _good_pose_info;
std
::
shared_ptr
<
BadOrGoodPoseInfo
>
sp_good_pose_info_
=
nullptr
;
std
::
shared_ptr
<
BadOrGoodPoseInfo
>
sp_bad_pose_info_
=
nullptr
;
};
}
// namespace hdmap
}
// namespace apollo
modules/map/tools/map_datachecker/server/alignment_agent.h
0 → 100644
浏览文件 @
dd765961
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once
#include <grpc++/grpc++.h>
#include <chrono>
#include <memory>
#include <thread>
#include <vector>
#include "modules/map/tools/map_datachecker/proto/collection_error_code.pb.h"
#include "modules/map/tools/map_datachecker/proto/collection_service.pb.h"
#include "modules/map/tools/map_datachecker/server/eight_route.h"
#include "modules/map/tools/map_datachecker/server/pose_collection_agent.h"
#include "modules/map/tools/map_datachecker/server/static_align.h"
namespace
apollo
{
namespace
hdmap
{
typedef
State
AlignmentAgentState
;
template
<
typename
ALIGNMENT_TYPE
,
typename
REQUEST_TYPE
,
typename
RESPONSE_TYPE
>
class
AlignmentAgent
{
public:
AlignmentAgent
(
std
::
shared_ptr
<
JSonConf
>
sp_conf
,
std
::
shared_ptr
<
PoseCollectionAgent
>
sp_pose_collection_agent
)
{
sp_conf_
=
sp_conf
;
sp_pose_collection_agent_
=
sp_pose_collection_agent
;
}
void
Reset
()
{
sp_alignment_
=
std
::
make_shared
<
ALIGNMENT_TYPE
>
(
sp_conf_
);
state_
=
AlignmentAgentState
::
IDLE
;
need_stop_
=
false
;
}
grpc
::
Status
ProcessGrpcRequest
(
grpc
::
ServerContext
*
context
,
REQUEST_TYPE
*
request
,
RESPONSE_TYPE
*
response
)
{
AINFO
<<
"AlignmentAgent request: "
<<
request
->
DebugString
();
switch
(
request
->
cmd
())
{
case
CmdType
::
START
:
AINFO
<<
"AlignmentAgent start"
;
AlignmentStart
(
request
,
response
);
break
;
case
CmdType
::
CHECK
:
AINFO
<<
"AlignmentAgent check"
;
AlignmentCheck
(
request
,
response
);
break
;
case
CmdType
::
STOP
:
AINFO
<<
"AlignmentAgent stop"
;
AlignmentStop
(
request
,
response
);
break
;
default:
response
->
set_code
(
ErrorCode
::
ERROR_REQUEST
);
response
->
set_progress
(
sp_alignment_
->
GetProgress
());
AERROR
<<
"command error"
;
}
AINFO
<<
"AlignmentAgent progress: "
<<
response
->
progress
();
return
grpc
::
Status
::
OK
;
}
int
AlignmentStart
(
REQUEST_TYPE
*
request
,
RESPONSE_TYPE
*
response
)
{
if
(
AlignmentAgentState
::
RUNNING
==
GetState
())
{
AINFO
<<
"AlignmentAgent is running. do need start again"
;
response
->
set_code
(
ErrorCode
::
ERROR_REPEATED_START
);
response
->
set_progress
(
0.0
);
return
0
;
}
Reset
();
AsyncStartAlignment
();
response
->
set_code
(
ErrorCode
::
SUCCESS
);
response
->
set_progress
(
0.0
);
return
0
;
}
int
AsyncStartAlignment
()
{
SetState
(
AlignmentAgentState
::
RUNNING
);
std
::
thread
alignment_thread
([
=
]()
{
sp_alignment_
->
SetStartTime
(
UnixtimeNow
());
AINFO
<<
"set state RUNNING"
;
while
(
!
need_stop_
&&
!
apollo
::
cyber
::
IsShutdown
())
{
std
::
shared_ptr
<
std
::
vector
<
FramePose
>>
sp_poses
=
GetPoses
();
if
(
sp_poses
==
nullptr
)
{
AINFO
<<
"error, pose pointer is null"
;
return
;
}
sp_alignment_
->
Process
(
*
sp_poses
);
ErrorCode
code
=
sp_alignment_
->
GetReturnState
();
if
(
code
==
ErrorCode
::
ERROR_VERIFY_NO_GNSSPOS
||
code
==
ErrorCode
::
ERROR_GNSS_SIGNAL_FAIL
)
{
AERROR
<<
"Some error occured, while loop will exit"
;
break
;
}
AINFO
<<
"get progress:"
<<
sp_alignment_
->
GetProgress
();
if
(
fabs
(
1
-
sp_alignment_
->
GetProgress
())
<
1e-8
)
{
AINFO
<<
"alignment progress reached 1.0, thread exit"
;
break
;
}
AINFO
<<
"sleep "
<<
sp_conf_
->
alignment_featch_pose_sleep
<<
" sec"
;
auto
seconds
=
std
::
chrono
::
seconds
(
sp_conf_
->
alignment_featch_pose_sleep
);
std
::
this_thread
::
sleep_for
(
seconds
);
}
stopped_
=
true
;
AINFO
<<
"Align thread complete"
;
});
alignment_thread
.
detach
();
return
0
;
}
std
::
shared_ptr
<
std
::
vector
<
FramePose
>>
GetPoses
()
const
{
if
(
sp_pose_collection_agent_
==
nullptr
)
{
return
nullptr
;
}
return
sp_pose_collection_agent_
->
GetPoses
();
}
int
AlignmentCheck
(
REQUEST_TYPE
*
request
,
RESPONSE_TYPE
*
response
)
{
if
(
AlignmentAgentState
::
IDLE
==
GetState
())
{
AINFO
<<
"AlignmentAgent is idle. this call will be refused"
;
response
->
set_code
(
ErrorCode
::
ERROR_CHECK_BEFORE_START
);
response
->
set_progress
(
0.0
);
return
0
;
}
if
(
sp_alignment_
==
nullptr
)
{
AINFO
<<
"sp_alignment_ is null, check later"
;
response
->
set_code
(
ErrorCode
::
SUCCESS
);
response
->
set_progress
(
0.0
);
return
0
;
}
ErrorCode
code
=
sp_alignment_
->
GetReturnState
();
double
progress
=
sp_alignment_
->
GetProgress
();
response
->
set_code
(
code
);
response
->
set_progress
(
progress
);
if
(
code
==
ErrorCode
::
ERROR_VERIFY_NO_GNSSPOS
||
code
==
ErrorCode
::
ERROR_GNSS_SIGNAL_FAIL
)
{
stopped_
=
true
;
SetState
(
AlignmentAgentState
::
IDLE
);
return
-
1
;
}
return
0
;
}
int
AlignmentStop
(
REQUEST_TYPE
*
request
,
RESPONSE_TYPE
*
response
)
{
response
->
set_code
(
ErrorCode
::
SUCCESS
);
if
(
sp_alignment_
==
nullptr
)
{
response
->
set_progress
(
0.0
);
}
else
{
response
->
set_progress
(
sp_alignment_
->
GetProgress
());
need_stop_
=
true
;
}
SetState
(
AlignmentAgentState
::
IDLE
);
return
0
;
}
void
SetState
(
AlignmentAgentState
state
)
{
state_
=
state
;
}
AlignmentAgentState
GetState
()
const
{
return
state_
;
}
private:
std
::
shared_ptr
<
JSonConf
>
sp_conf_
=
nullptr
;
std
::
shared_ptr
<
ALIGNMENT_TYPE
>
sp_alignment_
=
nullptr
;
std
::
shared_ptr
<
PoseCollectionAgent
>
sp_pose_collection_agent_
=
nullptr
;
AlignmentAgentState
state_
;
bool
need_stop_
;
bool
stopped_
;
};
}
// namespace hdmap
}
// namespace apollo
modules/map/tools/map_datachecker/server/channel_verify.cc
0 → 100644
浏览文件 @
dd765961
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/map/tools/map_datachecker/server/channel_verify.h"
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/filesystem.hpp>
#include <unordered_map>
#include "cyber/cyber.h"
#include "cyber/proto/record.pb.h"
#include "cyber/record/record_viewer.h"
namespace
apollo
{
namespace
hdmap
{
ChannelVerify
::
ChannelVerify
(
std
::
shared_ptr
<
JSonConf
>
sp_conf
)
:
sp_conf_
(
sp_conf
)
{
Reset
();
}
void
ChannelVerify
::
Reset
()
{
return_state_
=
ErrorCode
::
SUCCESS
;
checked_records_
.
clear
();
sp_vec_check_result_
=
std
::
make_shared
<
std
::
vector
<
OneRecordChannelCheckResult
>>
();
}
ErrorCode
ChannelVerify
::
Check
(
const
std
::
string
&
record_dir_or_record_full_path
)
{
std
::
vector
<
std
::
string
>
records_path
;
records_path
=
GetRecordsPath
(
record_dir_or_record_full_path
);
if
(
records_path
.
size
()
==
0
)
{
AINFO
<<
"have no data file to check"
;
return_state_
=
ErrorCode
::
ERROR_VERIFY_NO_RECORDERS
;
return
return_state_
;
}
IncrementalCheck
(
records_path
);
return_state_
=
ErrorCode
::
SUCCESS
;
return
return_state_
;
}
std
::
shared_ptr
<
std
::
vector
<
OneRecordChannelCheckResult
>>
ChannelVerify
::
get_check_result
()
const
{
return
sp_vec_check_result_
;
}
int
ChannelVerify
::
IncrementalCheck
(
const
std
::
vector
<
std
::
string
>&
records_path
)
{
std
::
vector
<
std
::
string
>
not_check_records_path
;
AINFO
<<
"all records path:"
;
for
(
size_t
i
=
0
;
i
<
records_path
.
size
();
++
i
)
{
AINFO
<<
"["
<<
i
<<
"]: "
<<
records_path
[
i
];
if
(
IsRecordFile
(
records_path
[
i
])
&&
!
IsRecordChecked
(
records_path
[
i
]))
{
not_check_records_path
.
push_back
(
records_path
[
i
]);
}
}
AINFO
<<
"not_check_records_path:"
;
for
(
size_t
i
=
0
;
i
<
not_check_records_path
.
size
();
++
i
)
{
AINFO
<<
"["
<<
i
<<
"]: "
<<
not_check_records_path
[
i
];
OneRecordChannelCheckResult
check_result
=
CheckRecordChannels
(
not_check_records_path
[
i
]);
if
(
check_result
.
record_path
.
empty
())
{
continue
;
}
sp_vec_check_result_
->
push_back
(
check_result
);
}
return
0
;
}
bool
ChannelVerify
::
IsRecordFile
(
const
std
::
string
&
record_path
)
const
{
if
(
!
boost
::
filesystem
::
exists
(
record_path
))
{
AINFO
<<
"path ["
<<
record_path
<<
"] does not exist"
;
return
false
;
}
if
(
!
boost
::
filesystem
::
is_regular_file
(
record_path
))
{
AINFO
<<
"path ["
<<
record_path
<<
"] is not a regular file"
;
return
false
;
}
// To avoid disk overhead caused by opening files twice, the real
// file checking is placed in the function [ChannelVerify::get_record_info]
return
true
;
}
std
::
vector
<
std
::
string
>
ChannelVerify
::
GetRecordsPath
(
const
std
::
string
&
record_dir_or_record_full_path
)
const
{
// record_dir_or_record_full_path is record fullpath or
// directory which contains some records
std
::
vector
<
std
::
string
>
records_path
;
// 1. check record_dir_or_record_full_path is valid or not
boost
::
filesystem
::
path
path
(
record_dir_or_record_full_path
);
if
(
!
boost
::
filesystem
::
exists
(
path
))
{
AINFO
<<
"record path ["
<<
record_dir_or_record_full_path
<<
"] does not exist"
;
return
records_path
;
}
if
(
IsRecordFile
(
record_dir_or_record_full_path
))
{
records_path
.
push_back
(
record_dir_or_record_full_path
);
}
else
if
(
boost
::
filesystem
::
is_directory
(
path
))
{
using
dit_t
=
boost
::
filesystem
::
directory_iterator
;
dit_t
end
;
for
(
dit_t
it
(
record_dir_or_record_full_path
);
it
!=
end
;
++
it
)
{
if
(
IsRecordFile
(
it
->
path
().
string
()))
{
records_path
.
push_back
(
it
->
path
().
string
());
}
}
}
return
records_path
;
}
bool
ChannelVerify
::
IsRecordChecked
(
const
std
::
string
&
record_path
)
{
return
!
checked_records_
.
insert
(
record_path
).
second
;
}
std
::
shared_ptr
<
CyberRecordInfo
>
ChannelVerify
::
GetRecordInfo
(
const
std
::
string
&
record_path
)
const
{
if
(
!
IsRecordFile
(
record_path
))
{
AINFO
<<
"get_record_info failed.["
<<
record_path
<<
"] is not record file"
;
return
nullptr
;
}
std
::
shared_ptr
<
CyberRecordInfo
>
sp_record_info
(
new
CyberRecordInfo
);
std
::
shared_ptr
<
apollo
::
cyber
::
record
::
RecordReader
>
sp_reader
=
std
::
make_shared
<
apollo
::
cyber
::
record
::
RecordReader
>
(
record_path
);
if
(
sp_reader
==
nullptr
||
!
sp_reader
->
IsValid
())
{
AINFO
<<
"open record ["
<<
record_path
<<
"] failed"
;
return
nullptr
;
}
std
::
shared_ptr
<
apollo
::
cyber
::
record
::
RecordViewer
>
sp_viewer
(
new
apollo
::
cyber
::
record
::
RecordViewer
(
sp_reader
));
sp_record_info
->
path
=
record_path
;
sp_record_info
->
start_time
=
sp_viewer
->
begin_time
();
sp_record_info
->
end_time
=
sp_viewer
->
end_time
();
sp_record_info
->
duration
=
static_cast
<
double
>
((
sp_viewer
->
end_time
()
-
sp_viewer
->
begin_time
()))
/
1e9
;
std
::
set
<
std
::
string
>
channel_list
=
sp_reader
->
GetChannelList
();
for
(
auto
it
=
channel_list
.
begin
();
it
!=
channel_list
.
end
();
++
it
)
{
const
std
::
string
&
channel_name
=
*
it
;
CyberRecordChannel
channel
;
channel
.
channel_name
=
channel_name
;
channel
.
msgnum
=
sp_reader
->
GetMessageNumber
(
channel_name
);
channel
.
msg_type
=
sp_reader
->
GetMessageType
(
channel_name
);
sp_record_info
->
channels
.
push_back
(
channel
);
}
return
sp_record_info
;
}
OneRecordChannelCheckResult
ChannelVerify
::
CheckRecordChannels
(
const
std
::
string
&
record_path
)
{
OneRecordChannelCheckResult
check_result
;
std
::
shared_ptr
<
CyberRecordInfo
>
sp_record_info
=
GetRecordInfo
(
record_path
);
if
(
sp_record_info
==
nullptr
)
{
return
check_result
;
}
std
::
vector
<
CyberRecordChannel
>&
channels
=
sp_record_info
->
channels
;
std
::
vector
<
std
::
pair
<
std
::
string
,
double
>>&
topic_list
=
sp_conf_
->
topic_list
;
check_result
.
record_path
=
record_path
;
check_result
.
start_time
=
sp_record_info
->
start_time
;
for
(
size_t
i
=
0
;
i
<
topic_list
.
size
();
++
i
)
{
std
::
string
&
channel_in_list
=
topic_list
[
i
].
first
;
double
channel_expected_rate
=
topic_list
[
i
].
second
;
bool
channel_in_list_found
=
false
;
size_t
j
=
0
;
for
(
j
=
0
;
j
<
channels
.
size
();
++
j
)
{
std
::
string
&
channel_in_record
=
channels
[
j
].
channel_name
;
if
(
channel_in_record
==
channel_in_list
)
{
channel_in_list_found
=
true
;
break
;
}
}
if
(
!
channel_in_list_found
)
{
// topic
AINFO
<<
record_path
<<
" lacks ["
<<
channel_in_list
<<
"]"
;
check_result
.
lack_channels
.
push_back
(
channel_in_list
);
}
else
{
// rate
double
actual_rate
=
static_cast
<
double
>
((
channels
[
j
].
msgnum
))
/
sp_record_info
->
duration
;
if
(
actual_rate
<
1e-8
)
{
actual_rate
=
0.0
;
AINFO
<<
"msgnum:"
<<
channels
[
j
].
msgnum
<<
",duration:"
<<
sp_record_info
->
duration
;
}
AINFO
<<
record_path
<<
" ["
<<
channel_in_list
<<
"] expected rate: "
<<
channel_expected_rate
<<
", actual rate: "
<<
actual_rate
;
if
(
actual_rate
<
channel_expected_rate
*
sp_conf_
->
topic_rate_tolerance
)
{
check_result
.
inadequate_rate
[
channel_in_list
]
=
std
::
make_pair
(
channel_expected_rate
,
actual_rate
);
}
}
}
return
check_result
;
}
ErrorCode
ChannelVerify
::
GetReturnState
()
const
{
return
return_state_
;
}
}
// namespace hdmap
}
// namespace apollo
modules/map/tools/map_datachecker/server/channel_verify.h
0 → 100644
浏览文件 @
dd765961
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once
#include <map>
#include <memory>
#include <set>
#include <string>
#include <utility>
#include <vector>
#include "modules/map/tools/map_datachecker/proto/collection_error_code.pb.h"
#include "modules/map/tools/map_datachecker/server/common.h"
namespace
apollo
{
namespace
hdmap
{
struct
CyberRecordChannel
{
std
::
string
channel_name
;
uint64_t
msgnum
;
std
::
string
msg_type
;
};
struct
CyberRecordInfo
{
std
::
string
path
;
double
duration
;
// the unit is seconds
uint64_t
start_time
;
uint64_t
end_time
;
std
::
vector
<
CyberRecordChannel
>
channels
;
};
struct
OneRecordChannelCheckResult
{
std
::
string
record_path
;
uint64_t
start_time
;
std
::
vector
<
std
::
string
>
lack_channels
;
// inadequate_rate: channel_name <---> (expected_rate, actual_rate)
std
::
map
<
std
::
string
,
std
::
pair
<
double
,
double
>>
inadequate_rate
;
};
typedef
std
::
shared_ptr
<
std
::
vector
<
OneRecordChannelCheckResult
>>
CheckedResult
;
typedef
std
::
vector
<
OneRecordChannelCheckResult
>::
iterator
CheckResultIterator
;
class
ChannelVerify
{
public:
explicit
ChannelVerify
(
std
::
shared_ptr
<
JSonConf
>
sp_conf
);
ErrorCode
Check
(
const
std
::
string
&
record_dir_or_record_full_path
);
std
::
shared_ptr
<
std
::
vector
<
OneRecordChannelCheckResult
>>
get_check_result
()
const
;
ErrorCode
GetReturnState
()
const
;
private:
bool
IsRecordFile
(
const
std
::
string
&
path
)
const
;
std
::
shared_ptr
<
CyberRecordInfo
>
GetRecordInfo
(
const
std
::
string
&
record_path
)
const
;
int
IncrementalCheck
(
const
std
::
vector
<
std
::
string
>&
records_path
);
std
::
vector
<
std
::
string
>
GetRecordsPath
(
const
std
::
string
&
record_dir_or_record_full_path
)
const
;
bool
IsRecordChecked
(
const
std
::
string
&
record_path
);
OneRecordChannelCheckResult
CheckRecordChannels
(
const
std
::
string
&
record_path
);
void
Reset
();
private:
std
::
shared_ptr
<
JSonConf
>
sp_conf_
=
nullptr
;
CheckedResult
sp_vec_check_result_
=
nullptr
;
ErrorCode
return_state_
;
std
::
set
<
std
::
string
>
checked_records_
;
};
}
// namespace hdmap
}
// namespace apollo
modules/map/tools/map_datachecker/server/channel_verify_agent.cc
0 → 100644
浏览文件 @
dd765961
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/map/tools/map_datachecker/server/channel_verify_agent.h"
#include <chrono>
#include <map>
#include <string>
#include <thread>
#include <utility>
#include <vector>
namespace
apollo
{
namespace
hdmap
{
ChannelVerifyAgent
::
ChannelVerifyAgent
(
std
::
shared_ptr
<
JSonConf
>
sp_conf
)
{
sp_conf_
=
sp_conf
;
sp_channel_checker_
=
nullptr
;
sp_check_result_
=
nullptr
;
}
void
ChannelVerifyAgent
::
Reset
()
{
std
::
lock_guard
<
std
::
mutex
>
guard
(
stop_mutex_
);
need_stop_
=
false
;
stopped_
=
false
;
sp_channel_checker_
=
std
::
make_shared
<
ChannelVerify
>
(
sp_conf_
);
sp_check_result_
=
nullptr
;
SetState
(
ChannelVerifyAgentState
::
IDLE
);
}
grpc
::
Status
ChannelVerifyAgent
::
ProcessGrpcRequest
(
grpc
::
ServerContext
*
context
,
ChannelVerifyRequest
*
request
,
ChannelVerifyResponse
*
response
)
{
AINFO
<<
"ChannelVerifyAgent Request: "
<<
request
->
DebugString
();
switch
(
request
->
cmd
())
{
case
CmdType
::
START
:
AINFO
<<
"ChannelVerifyAgent start"
;
StartCheck
(
request
,
response
);
break
;
case
CmdType
::
CHECK
:
AINFO
<<
"ChannelVerifyAgent check"
;
CheckResult
(
request
,
response
);
break
;
case
CmdType
::
STOP
:
AINFO
<<
"ChannelVerifyAgent stop"
;
StopCheck
(
request
,
response
);
break
;
default:
response
->
set_code
(
ErrorCode
::
ERROR_REQUEST
);
AERROR
<<
"command error"
;
}
AINFO
<<
"ChannelVerifyAgent progress: "
<<
response
->
DebugString
();
return
grpc
::
Status
::
OK
;
}
void
ChannelVerifyAgent
::
StartCheck
(
ChannelVerifyRequest
*
request
,
ChannelVerifyResponse
*
response
)
{
if
(
GetState
()
==
ChannelVerifyAgentState
::
RUNNING
)
{
AINFO
<<
"ChannelVerify is RUNNING, do not need start again"
;
response
->
set_code
(
ErrorCode
::
ERROR_REPEATED_START
);
return
;
}
Reset
();
std
::
string
records_path
=
request
->
path
();
AsyncCheck
(
records_path
);
response
->
set_code
(
ErrorCode
::
SUCCESS
);
}
void
ChannelVerifyAgent
::
AsyncCheck
(
const
std
::
string
&
records_path
)
{
SetState
(
ChannelVerifyAgentState
::
RUNNING
);
std
::
thread
doctor_strange
([
=
]()
{
check_thread_id_
=
std
::
this_thread
::
get_id
();
int
wait_sec
=
sp_conf_
->
channel_check_trigger_gap
;
while
(
true
)
{
{
std
::
lock_guard
<
std
::
mutex
>
guard
(
stop_mutex_
);
if
(
need_stop_
)
{
break
;
}
DoCheck
(
records_path
);
AINFO
<<
"thread check done"
;
}
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
seconds
(
wait_sec
));
if
(
std
::
this_thread
::
get_id
()
!=
check_thread_id_
)
{
break
;
}
}
stopped_
=
true
;
});
doctor_strange
.
detach
();
AINFO
<<
"ChannelVerifyAgent::async_check exit"
;
}
void
ChannelVerifyAgent
::
DoCheck
(
const
std
::
string
&
records_path
)
{
if
(
sp_channel_checker_
==
nullptr
)
{
sp_channel_checker_
=
std
::
make_shared
<
ChannelVerify
>
(
sp_conf_
);
}
SetState
(
ChannelVerifyAgentState
::
RUNNING
);
sp_channel_checker_
->
Check
(
records_path
);
sp_check_result_
=
sp_channel_checker_
->
get_check_result
();
}
int
ChannelVerifyAgent
::
AddTopicLack
(
VerifyResult
*
result
,
const
std
::
string
&
record_path
,
std
::
vector
<
std
::
string
>
const
&
lack_channels
)
{
TopicResult
*
topics
=
result
->
mutable_topics
();
for
(
const
std
::
string
&
channel
:
lack_channels
)
{
topics
->
add_topic_lack
(
channel
);
AINFO
<<
record_path
<<
" lack topic: "
<<
channel
;
}
return
static_cast
<
int
>
(
lack_channels
.
size
());
}
FrameRate
*
ChannelVerifyAgent
::
FindRates
(
VerifyResult
*
result
,
const
std
::
string
&
channel
)
{
int
rates_size
=
result
->
rates_size
();
for
(
const
FrameRate
&
rates
:
result
)
{
if
(
rates
.
topic
()
==
channel
)
{
return
result
->
mutable_rates
(
i
);
}
}
return
nullptr
;
}
int
ChannelVerifyAgent
::
AddInadequateRate
(
VerifyResult
*
result
,
std
::
string
const
&
record_path
,
std
::
map
<
std
::
string
,
std
::
pair
<
double
,
double
>>
const
&
inadequate_rate
)
{
for
(
auto
it
=
inadequate_rate
.
begin
();
it
!=
inadequate_rate
.
end
();
++
it
)
{
const
std
::
string
&
channel
=
it
->
first
;
double
expected_rate
=
it
->
second
.
first
;
double
current_rate
=
it
->
second
.
second
;
FrameRate
*
rate
=
FindRates
(
result
,
channel
);
if
(
rate
==
nullptr
)
{
rate
=
result
->
add_rates
();
rate
->
add_bad_record_name
(
record_path
);
rate
->
set_topic
(
channel
);
rate
->
set_expected_rate
(
expected_rate
);
rate
->
set_current_rate
(
current_rate
);
}
else
{
rate
->
set_current_rate
(
current_rate
);
rate
->
add_bad_record_name
(
record_path
);
}
}
return
result
->
rates_size
();
}
void
ChannelVerifyAgent
::
CheckResult
(
ChannelVerifyRequest
*
request
,
ChannelVerifyResponse
*
response
)
{
if
(
GetState
()
==
ChannelVerifyAgentState
::
IDLE
)
{
AINFO
<<
"ChannelVerify is not RUNNING, it should start first"
;
response
->
set_code
(
ErrorCode
::
ERROR_CHECK_BEFORE_START
);
return
;
}
if
(
sp_channel_checker_
==
nullptr
||
sp_check_result_
==
nullptr
)
{
AINFO
<<
"check result is null. check later"
;
response
->
set_code
(
ErrorCode
::
SUCCESS
);
return
;
}
if
(
sp_channel_checker_
->
GetReturnState
()
!=
ErrorCode
::
SUCCESS
)
{
response
->
set_code
(
sp_channel_checker_
->
GetReturnState
());
return
;
}
response
->
set_code
(
ErrorCode
::
SUCCESS
);
VerifyResult
*
result
=
response
->
mutable_result
();
for
(
CheckResultIterator
it
=
sp_check_result_
->
begin
();
it
!=
sp_check_result_
->
end
();
++
it
)
{
int
res
=
0
;
// write rate
res
=
AddInadequateRate
(
result
,
it
->
record_path
,
it
->
inadequate_rate
);
if
(
res
>
0
)
{
response
->
set_code
(
ErrorCode
::
ERROR_CHANNEL_VERIFY_RATES_ABNORMAL
);
}
// write topic lack
res
=
AddTopicLack
(
result
,
it
->
record_path
,
it
->
lack_channels
);
if
(
res
>
0
)
{
response
->
set_code
(
ErrorCode
::
ERROR_CHANNEL_VERIFY_TOPIC_LACK
);
}
}
}
void
ChannelVerifyAgent
::
StopCheck
(
ChannelVerifyRequest
*
request
,
ChannelVerifyResponse
*
response
)
{
std
::
lock_guard
<
std
::
mutex
>
guard
(
stop_mutex_
);
need_stop_
=
true
;
response
->
set_code
(
ErrorCode
::
SUCCESS
);
SetState
(
ChannelVerifyAgentState
::
IDLE
);
VerifyResult
*
result
=
response
->
mutable_result
();
if
(
sp_check_result_
==
nullptr
)
{
return
;
}
for
(
CheckResultIterator
it
=
sp_check_result_
->
begin
();
it
!=
sp_check_result_
->
end
();
++
it
)
{
int
res
=
0
;
// write rate
res
=
AddInadequateRate
(
result
,
it
->
record_path
,
it
->
inadequate_rate
);
if
(
res
>
0
)
{
response
->
set_code
(
ErrorCode
::
ERROR_CHANNEL_VERIFY_RATES_ABNORMAL
);
}
// write topic lack
res
=
AddTopicLack
(
result
,
it
->
record_path
,
it
->
lack_channels
);
if
(
res
>
0
)
{
response
->
set_code
(
ErrorCode
::
ERROR_CHANNEL_VERIFY_TOPIC_LACK
);
}
}
}
void
ChannelVerifyAgent
::
SetState
(
ChannelVerifyAgentState
state
)
{
state_
=
state
;
}
ChannelVerifyAgentState
ChannelVerifyAgent
::
GetState
()
const
{
return
state_
;
}
}
// namespace hdmap
}
// namespace apollo
modules/map/tools/map_datachecker/server/channel_verify_agent.h
0 → 100644
浏览文件 @
dd765961
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once
#include <grpc++/grpc++.h>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <utility>
#include <vector>
#include "modules/map/tools/map_datachecker/proto/collection_error_code.pb.h"
#include "modules/map/tools/map_datachecker/proto/collection_service.pb.h"
#include "modules/map/tools/map_datachecker/server/channel_verify.h"
namespace
apollo
{
namespace
hdmap
{
enum
class
ChannelVerifyAgentState
{
IDLE
,
RUNNING
};
class
ChannelVerifyAgent
{
public:
explicit
ChannelVerifyAgent
(
std
::
shared_ptr
<
JSonConf
>
sp_conf
);
grpc
::
Status
ProcessGrpcRequest
(
grpc
::
ServerContext
*
context
,
ChannelVerifyRequest
*
request
,
ChannelVerifyResponse
*
response
);
private:
void
StartCheck
(
ChannelVerifyRequest
*
request
,
ChannelVerifyResponse
*
response
);
void
AsyncCheck
(
const
std
::
string
&
records_path
);
void
DoCheck
(
const
std
::
string
&
records_path
);
void
CheckResult
(
ChannelVerifyRequest
*
request
,
ChannelVerifyResponse
*
response
);
void
StopCheck
(
ChannelVerifyRequest
*
request
,
ChannelVerifyResponse
*
response
);
void
Reset
();
void
SetState
(
ChannelVerifyAgentState
state
);
ChannelVerifyAgentState
GetState
()
const
;
int
AddTopicLack
(
VerifyResult
*
result
,
const
std
::
string
&
record_path
,
std
::
vector
<
std
::
string
>
const
&
lack_channels
);
int
AddInadequateRate
(
VerifyResult
*
result
,
std
::
string
const
&
record_path
,
std
::
map
<
std
::
string
,
std
::
pair
<
double
,
double
>>
const
&
inadequate_rate
);
FrameRate
*
FindRates
(
VerifyResult
*
result
,
const
std
::
string
&
channel
);
private:
ChannelVerifyAgentState
state_
;
std
::
mutex
stop_mutex_
;
bool
need_stop_
;
bool
stopped_
;
std
::
shared_ptr
<
JSonConf
>
sp_conf_
=
nullptr
;
std
::
shared_ptr
<
ChannelVerify
>
sp_channel_checker_
=
nullptr
;
CheckedResult
sp_check_result_
=
nullptr
;
std
::
thread
::
id
check_thread_id_
;
};
}
// namespace hdmap
}
// namespace apollo
modules/map/tools/map_datachecker/server/eight_route.cc
0 → 100644
浏览文件 @
dd765961
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/map/tools/map_datachecker/server/eight_route.h"
#include <cmath>
#include <vector>
namespace
apollo
{
namespace
hdmap
{
EightRoute
::
EightRoute
(
std
::
shared_ptr
<
JSonConf
>
sp_conf
)
:
Alignment
(
sp_conf
)
{
Reset
();
}
void
EightRoute
::
Reset
()
{
progress_
=
0.0
;
last_progress_
=
0
;
}
bool
EightRoute
::
IsEightRoutePose
(
const
std
::
vector
<
FramePose
>&
poses
,
int
pose_index
)
{
if
(
poses
.
size
()
==
0
||
pose_index
<=
0
||
pose_index
>=
static_cast
<
int
>
(
poses
.
size
()))
{
AINFO
<<
"params error, poses size: "
<<
poses
.
size
()
<<
", pose_index: "
<<
pose_index
;
return
true
;
}
double
yaw
=
GetYaw
(
poses
[
pose_index
-
1
].
tx
,
poses
[
pose_index
-
1
].
ty
,
poses
[
pose_index
].
tx
,
poses
[
pose_index
].
ty
);
double
yaw_diff
=
fabs
(
last_yaw_
-
yaw
);
last_yaw_
=
yaw
;
yaw_diff
=
yaw_diff
<
180
?
yaw_diff
:
360
-
yaw_diff
;
double
xdiff
=
poses
[
pose_index
].
tx
-
poses
[
pose_index
-
1
].
tx
;
double
ydiff
=
poses
[
pose_index
].
ty
-
poses
[
pose_index
-
1
].
ty
;
double
zdiff
=
poses
[
pose_index
].
tz
-
poses
[
pose_index
-
1
].
tz
;
double
dist
=
std
::
sqrt
(
xdiff
*
xdiff
+
ydiff
*
ydiff
+
zdiff
*
zdiff
);
double
during
=
poses
[
pose_index
].
time_stamp
-
poses
[
pose_index
-
1
].
time_stamp
;
if
(
during
<
0
)
{
AINFO
<<
"skip back pose is bad pose"
;
return
false
;
}
double
vel
=
dist
/
during
;
AINFO
<<
std
::
to_string
(
poses
[
pose_index
].
time_stamp
)
<<
", yaw_diff:"
<<
yaw_diff
<<
", dist: "
<<
dist
<<
", during: "
<<
during
<<
", vel: "
<<
vel
;
if
(
yaw_diff
>
sp_conf_
->
eight_angle
&&
vel
>
sp_conf_
->
eight_vel
)
{
return
true
;
}
return
false
;
}
double
EightRoute
::
GetGoodPoseDuring
()
{
if
(
sp_good_pose_info_
==
nullptr
||
sp_good_pose_info_
->
start_time
<
0
||
sp_good_pose_info_
->
end_time
<
0
)
{
return
0.0
;
}
return
sp_good_pose_info_
->
end_time
-
sp_good_pose_info_
->
start_time
;
}
double
EightRoute
::
GetEightRouteProgress
(
const
std
::
vector
<
FramePose
>&
poses
)
{
int
size
=
static_cast
<
int
>
(
poses
.
size
());
int
start_index
=
TimeToIndex
(
poses
,
start_time_
);
// select first good pose
while
(
start_index
<
size
)
{
if
(
IsGoodPose
(
poses
,
start_index
)
&&
IsEightRoutePose
(
poses
,
start_index
))
{
AINFO
<<
"find first good pose.index:"
<<
start_index
;
break
;
}
++
start_index
;
}
if
(
start_index
>=
size
)
{
AINFO
<<
"not find first good pose, start_time: "
<<
std
::
to_string
(
start_time_
)
<<
", start_index: "
<<
start_index
<<
", pose size: "
<<
size
;
return
0.0
;
}
if
(
start_index
+
1
>=
size
)
{
AINFO
<<
"not have enough poses, wait for a moment"
;
return
0.0
;
}
last_yaw_
=
GetYaw
(
poses
[
start_index
].
tx
,
poses
[
start_index
].
ty
,
poses
[
start_index
+
1
].
tx
,
poses
[
start_index
+
1
].
ty
);
int
not_eight_count
=
0
;
for
(
int
i
=
start_index
+
2
;
i
<
size
;
++
i
)
{
if
(
!
IsGoodPose
(
poses
,
i
))
{
AINFO
<<
"not good pose"
;
return
0.0
;
}
if
(
!
IsEightRoutePose
(
poses
,
i
))
{
++
not_eight_count
;
AINFO
<<
"not eight route pose"
;
if
(
not_eight_count
>
sp_conf_
->
eight_bad_pose_tolerance
)
{
AINFO
<<
"not-eight pose count reached upper limitation"
;
return_state_
=
ErrorCode
::
ERROR_NOT_EIGHT_ROUTE
;
return
0.0
;
}
}
else
{
not_eight_count
=
0
;
}
AINFO
<<
"good pose"
;
UpdateGoodPoseInfo
(
poses
[
i
]);
// ClearBadPoseInfo();
}
double
eight_route_during
=
GetGoodPoseDuring
();
if
(
eight_route_during
<
1e-8
)
{
AINFO
<<
"num of eight route good pose too small, during: "
<<
eight_route_during
;
return_state_
=
ErrorCode
::
SUCCESS
;
return
0.0
;
}
return_state_
=
ErrorCode
::
SUCCESS
;
double
progress
=
eight_route_during
/
sp_conf_
->
eight_duration
;
if
(
progress
>=
1.0
)
{
progress
=
1.0
;
}
ClearGoodPoseInfo
();
return
progress
;
}
ErrorCode
EightRoute
::
Process
(
const
std
::
vector
<
FramePose
>&
poses
)
{
AINFO
<<
"[EightRoute::process] begin"
;
size_t
size
=
poses
.
size
();
if
(
size
<=
1
)
{
return_state_
=
ErrorCode
::
ERROR_VERIFY_NO_GNSSPOS
;
return
return_state_
;
}
progress_
=
GetEightRouteProgress
(
poses
);
if
(
return_state_
!=
ErrorCode
::
SUCCESS
)
{
AINFO
<<
"get_eight_route_progress failed."
;
return
return_state_
;
}
if
(
progress_
<
last_progress_
)
{
return_state_
=
ErrorCode
::
ERROR_NOT_EIGHT_ROUTE
;
return
return_state_
;
}
AINFO
<<
"[EightRoute::process] end, progress:"
<<
progress_
;
return_state_
=
ErrorCode
::
SUCCESS
;
return
return_state_
;
}
double
EightRoute
::
GetProgress
()
const
{
return
progress_
;
}
}
// namespace hdmap
}
// namespace apollo
modules/map/tools/map_datachecker/server/eight_route.h
0 → 100644
浏览文件 @
dd765961
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once
#include <grpc++/grpc++.h>
#include <memory>
#include <vector>
#include "cyber/cyber.h"
#include "modules/map/tools/map_datachecker/server/alignment.h"
#include "modules/map/tools/map_datachecker/server/common.h"
#include "modules/map/tools/map_datachecker/server/worker_gflags.h"
namespace
apollo
{
namespace
hdmap
{
// TODO(yuanyijun): change EightRoute to FigureEight
class
EightRoute
:
public
Alignment
{
public:
explicit
EightRoute
(
std
::
shared_ptr
<
JSonConf
>
sp_conf
);
ErrorCode
Process
(
const
std
::
vector
<
FramePose
>&
poses
);
double
GetProgress
()
const
;
private:
void
Reset
();
bool
IsEightRoutePose
(
const
std
::
vector
<
FramePose
>&
poses
,
int
pose_index
);
double
GetGoodPoseDuring
();
double
GetEightRouteProgress
(
const
std
::
vector
<
FramePose
>&
poses
);
private:
double
progress_
;
double
last_yaw_
;
};
}
// namespace hdmap
}
// namespace apollo
modules/map/tools/map_datachecker/server/laps_checker.cc
0 → 100644
浏览文件 @
dd765961
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/map/tools/map_datachecker/server/laps_checker.h"
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <limits>
namespace
apollo
{
namespace
hdmap
{
LapsChecker
::
LapsChecker
(
const
std
::
vector
<
FramePose
>
&
poses
,
int
laps_to_check
,
std
::
shared_ptr
<
JSonConf
>
sp_conf
)
:
poses_
(
poses
),
sp_conf_
(
sp_conf
)
{
laps_to_check_
=
laps_to_check
;
maxx_
=
0.0
;
maxy_
=
0.0
;
minx_
=
0.0
;
miny_
=
0.0
;
possible_max_laps_
=
(
laps_to_check_
+
1
)
*
10
;
confidence_
.
resize
(
possible_max_laps_
+
1
,
0.0
);
finished_
=
false
;
return_state_
=
ErrorCode
::
SUCCESS
;
AINFO
<<
"instance has "
<<
poses
.
size
()
<<
" poses"
;
AINFO
<<
"confidence size: "
<<
possible_max_laps_
+
1
;
}
int
LapsChecker
::
SetProgress
(
double
p
)
{
progress_
=
p
;
return
0
;
}
double
LapsChecker
::
GetProgress
()
const
{
return
progress_
;
}
size_t
LapsChecker
::
GetLap
()
const
{
return
lap_
;
}
double
LapsChecker
::
GetConfidence
()
{
double
res
=
0.0
;
lap_
=
laps_to_check_
;
for
(
size_t
i
=
0
;
i
<
confidence_
.
size
();
++
i
)
{
AINFO
<<
"confidence["
<<
i
<<
"]: "
<<
std
::
to_string
(
confidence_
[
i
]);
}
AINFO
<<
"laps to check: "
<<
laps_to_check_
;
for
(
size_t
i
=
laps_to_check_
;
i
<
confidence_
.
size
();
++
i
)
{
res
+=
confidence_
[
i
];
}
AINFO
<<
"current confidence: "
<<
res
<<
", confidence thresh:"
<<
sp_conf_
->
laps_rate_thresh
;
if
(
res
<
sp_conf_
->
laps_rate_thresh
)
{
if
(
confidence_
.
size
()
==
0
)
{
AINFO
<<
"some problems induce lap problem"
;
return
0.0
;
}
res
=
confidence_
[
0
];
lap_
=
0
;
for
(
size_t
i
=
1
;
i
<
confidence_
.
size
();
++
i
)
{
if
(
i
==
laps_to_check_
)
{
continue
;
}
if
(
confidence_
[
i
]
>
res
)
{
lap_
=
i
;
res
=
confidence_
[
i
];
}
}
}
return
res
;
}
ErrorCode
LapsChecker
::
Check
()
{
if
(
poses_
.
size
()
==
0
)
{
return_state_
=
ErrorCode
::
ERROR_VERIFY_NO_GNSSPOS
;
return
return_state_
;
}
DoCheck
();
finished_
=
true
;
return
return_state_
;
}
void
LapsChecker
::
DoCheck
()
{
AINFO
<<
"do_check"
;
SetProgress
(
0.0
);
int
ret
=
0
;
AINFO
<<
"check->check_params"
;
ret
=
CheckParams
();
if
(
ret
<
0
)
{
AINFO
<<
"check_params failed"
;
}
SetProgress
(
0.1
);
AINFO
<<
"check->setup_grids_map"
;
ret
=
SetupGridsMap
();
if
(
ret
<
0
)
{
AINFO
<<
"setup_grids_map failed"
;
}
SetProgress
(
0.5
);
AINFO
<<
"check->setup_grids_map done"
;
AINFO
<<
"check->check_laps"
;
ret
=
CheckLaps
();
if
(
ret
<
0
)
{
AINFO
<<
"check_laps failed"
;
}
SetProgress
(
1.0
);
AINFO
<<
"check->check_laps done"
;
AINFO
<<
"do_check done"
;
}
int
LapsChecker
::
CheckParams
()
{
int
n_pose
=
static_cast
<
int
>
(
poses_
.
size
());
if
(
n_pose
<
sp_conf_
->
laps_frames_thresh
)
{
return
-
1
;
}
return
0
;
}
int
LapsChecker
::
SetupGridsMap
()
{
AINFO
<<
"setup_grids_map->get_min_max"
;
GetMinMax
();
AINFO
<<
"setup_grids_map->do_setup_grids_map"
;
int
ret
=
DoSetupGridsMap
();
if
(
ret
<
0
)
{
AINFO
<<
"do_setup_grids_map failed"
;
return
-
1
;
}
AINFO
<<
"setup_grids_map done"
;
return
0
;
}
int
LapsChecker
::
CheckLaps
()
{
int
height
=
static_cast
<
int
>
(
grids_map_
.
size
());
if
(
height
<=
2
||
height
>
1000000
)
{
AINFO
<<
"grids_map_ size error. height = "
<<
height
;
return
-
1
;
}
int
width
=
static_cast
<
int
>
(
grids_map_
[
0
].
size
());
if
(
width
<=
2
||
width
>=
1000000
)
{
AINFO
<<
"grids_map_ size error. width = "
<<
width
;
return
-
1
;
}
for
(
int
y
=
0
;
y
<
height
;
y
++
)
{
for
(
int
x
=
0
;
x
<
width
;
x
++
)
{
Grid
&
grid
=
grids_map_
[
y
][
x
];
size_t
t_size
=
grid
.
size
();
if
(
t_size
==
0
)
{
continue
;
}
for
(
size_t
i
=
0
;
i
<
t_size
;
++
i
)
{
std
::
vector
<
double
>
stamps
;
GatherTimestamps
(
&
stamps
,
grid
[
i
].
alpha
,
x
,
y
);
if
(
stamps
.
size
()
==
0
)
{
continue
;
}
std
::
sort
(
stamps
.
begin
(),
stamps
.
end
());
double
thresh_in_sec
=
sp_conf_
->
laps_time_err_thresh
*
60
;
size_t
segment
=
1
;
for
(
size_t
j
=
1
;
j
<
stamps
.
size
();
++
j
)
{
if
(
stamps
[
j
]
-
stamps
[
j
-
1
]
>
thresh_in_sec
)
{
segment
++
;
}
}
if
(
segment
<=
possible_max_laps_
)
{
confidence_
[
segment
]
+=
1
;
}
}
}
}
double
all
=
0.0
;
for
(
size_t
i
=
0
;
i
<
confidence_
.
size
();
++
i
)
{
all
+=
confidence_
[
i
];
}
if
(
all
==
0.0
)
{
AINFO
<<
"there seems no poses"
;
return
-
1
;
}
AINFO
<<
"confidence size: "
<<
confidence_
.
size
();
for
(
size_t
i
=
0
;
i
<
confidence_
.
size
();
++
i
)
{
confidence_
[
i
]
/=
all
;
}
return
0
;
}
int
LapsChecker
::
GatherTimestamps
(
std
::
vector
<
double
>
*
sp_stamps
,
double
alpha
,
int
center_x
,
int
center_y
)
{
int
search_d
=
sp_conf_
->
laps_search_diameter
;
if
((
search_d
&
1
)
==
0
)
{
AINFO
<<
"laps_search_diameter should be an odd"
;
return
-
1
;
}
int
search_r
=
(
search_d
>>
1
);
size_t
height
=
grids_map_
.
size
(),
width
=
grids_map_
[
0
].
size
();
std
::
vector
<
double
>
&
stamps
=
*
sp_stamps
;
stamps
.
clear
();
const
size_t
start_y
=
std
::
max
(
0
,
center_y
-
search_r
);
const
size_t
end_y
=
std
::
min
(
static_cast
<
int
>
(
height
)
-
1
,
center_y
+
search_r
);
const
size_t
start_x
=
std
::
max
(
0
,
center_x
-
search_r
);
const
size_t
end_x
=
std
::
min
(
static_cast
<
int
>
(
width
)
-
1
,
center_x
+
search_r
);
for
(
size_t
y
=
start_y
;
y
<=
end_y
;
y
++
)
{
for
(
size_t
x
=
start_x
;
x
<=
end_x
;
x
++
)
{
Grid
&
grid
=
grids_map_
[
y
][
x
];
for
(
size_t
i
=
0
;
i
<
grid
.
size
();
++
i
)
{
if
(
std
::
abs
(
alpha
-
grid
[
i
].
alpha
)
<
sp_conf_
->
laps_alpha_err_thresh
)
{
std
::
vector
<
int
>
&
idxs
=
grid
[
i
].
idxs
;
for
(
size_t
j
=
0
;
j
<
idxs
.
size
();
++
j
)
{
if
(
idxs
[
j
]
>=
static_cast
<
int
>
(
poses_
.
size
()))
{
AINFO
<<
"index error, index: "
<<
idxs
[
j
]
<<
", pose size: "
<<
poses_
.
size
();
}
else
{
stamps
.
push_back
(
poses_
[
idxs
[
j
]].
time_stamp
);
}
}
}
}
}
}
return
0
;
}
int
LapsChecker
::
GetMinMax
()
{
minx_
=
std
::
numeric_limits
<
double
>::
max
();
miny_
=
std
::
numeric_limits
<
double
>::
max
();
maxx_
=
std
::
numeric_limits
<
double
>::
min
();
maxy_
=
std
::
numeric_limits
<
double
>::
min
();
size_t
size
=
poses_
.
size
();
AINFO
<<
"get_min_max pose size: "
<<
size
;
for
(
size_t
i
=
0
;
i
<
size
;
++
i
)
{
double
tx
=
poses_
[
i
].
tx
,
ty
=
poses_
[
i
].
ty
;
if
(
tx
<
minx_
)
{
minx_
=
tx
;
}
if
(
tx
>
maxx_
)
{
maxx_
=
tx
;
}
if
(
ty
<
miny_
)
{
miny_
=
ty
;
}
if
(
ty
>
maxy_
)
{
maxy_
=
ty
;
}
}
return
0
;
}
int
LapsChecker
::
DoSetupGridsMap
()
{
size_t
width
=
size_t
(
maxx_
-
minx_
+
1
);
size_t
height
=
size_t
(
maxy_
-
miny_
+
1
);
AINFO
<<
"grid map width: "
<<
width
<<
", height: "
<<
height
;
size_t
size
=
poses_
.
size
();
if
(
1
>=
size
||
0
==
height
||
0
==
width
||
height
>
1000000
||
width
>
1000000
)
{
AINFO
<<
"pose size: "
<<
size
<<
", height: "
<<
height
<<
", width: "
<<
width
;
AINFO
<<
"pose size error or grid map size error"
;
return
-
1
;
}
grids_map_
.
resize
(
height
);
for
(
size_t
i
=
0
;
i
<
height
;
++
i
)
{
grids_map_
[
i
].
resize
(
width
);
}
// first pose can not be used
for
(
size_t
i
=
1
;
i
<
size
;
++
i
)
{
int
x
=
static_cast
<
int
>
(
poses_
[
i
].
tx
-
minx_
);
int
y
=
static_cast
<
int
>
(
poses_
[
i
].
ty
-
miny_
);
PutPoseToGrid
(
static_cast
<
int
>
(
i
),
y
,
x
);
PutPoseToNeighborGrid
(
static_cast
<
int
>
(
i
));
}
return
0
;
}
double
LapsChecker
::
CalcAlpha
(
int
pose_index
)
{
double
vecx
=
poses_
[
pose_index
].
tx
-
poses_
[
pose_index
-
1
].
tx
;
double
vecy
=
poses_
[
pose_index
].
ty
-
poses_
[
pose_index
-
1
].
ty
;
double
alpha
=
acos
(
vecx
/
sqrt
(
vecx
*
vecx
+
vecy
*
vecy
))
*
180
/
M_PI
;
if
(
alpha
>
0
)
{
return
alpha
;
}
return
360
+
alpha
;
}
int
LapsChecker
::
PutPoseToGrid
(
int
pose_index
,
int
grid_y
,
int
grid_x
)
{
if
(
pose_index
<=
0
)
{
return
0
;
}
double
alpha
=
CalcAlpha
(
pose_index
);
if
(
std
::
isnan
(
alpha
))
{
AERROR
<<
"ignore static pose "
<<
pose_index
;
return
0
;
}
Grid
&
grid
=
grids_map_
[
grid_y
][
grid_x
];
size_t
t_size
=
grid
.
size
();
for
(
size_t
j
=
0
;
j
<
t_size
;
++
j
)
{
if
(
std
::
abs
(
alpha
-
grid
[
j
].
alpha
)
<
sp_conf_
->
laps_alpha_err_thresh
)
{
grid
[
j
].
idxs
.
push_back
(
pose_index
);
grid
[
j
].
alpha
=
(
grid
[
j
].
alpha
+
alpha
)
/
2
;
return
0
;
}
}
GridMeta
gm
;
gm
.
alpha
=
alpha
;
gm
.
idxs
=
{
pose_index
};
grid
.
push_back
(
gm
);
return
0
;
}
int
LapsChecker
::
PutPoseToNeighborGrid
(
int
pose_index
)
{
if
(
pose_index
<=
0
)
{
return
0
;
}
std
::
vector
<
int
>
x
,
y
;
GetPassedGrid
(
pose_index
,
&
x
,
&
y
);
for
(
size_t
i
=
0
;
i
<
x
.
size
();
++
i
)
{
PutPoseToGrid
(
pose_index
,
y
[
i
],
x
[
i
]);
}
return
0
;
}
int
LapsChecker
::
GetPassedGrid
(
int
pose_index
,
std
::
vector
<
int
>
*
sp_grid_x
,
std
::
vector
<
int
>
*
sp_grid_y
)
{
if
(
pose_index
<=
0
)
{
return
0
;
}
std
::
vector
<
int
>
&
grid_x
=
*
sp_grid_x
;
std
::
vector
<
int
>
&
grid_y
=
*
sp_grid_y
;
grid_x
.
clear
();
grid_y
.
clear
();
double
x
=
poses_
[
pose_index
].
tx
-
minx_
;
double
y
=
poses_
[
pose_index
].
ty
-
miny_
;
double
last_x
=
poses_
[
pose_index
-
1
].
tx
-
minx_
;
double
last_y
=
poses_
[
pose_index
-
1
].
ty
-
miny_
;
if
(
std
::
abs
(
x
-
last_x
)
<
1e-6
)
{
// current trace is vertical
int
start_y
=
static_cast
<
int
>
(
std
::
min
(
y
,
last_y
))
+
1
;
int
end_y
=
static_cast
<
int
>
(
std
::
max
(
y
,
last_y
));
int
start_x
=
static_cast
<
int
>
(
x
);
while
((
start_y
++
)
<
end_y
)
{
grid_x
.
push_back
(
start_x
);
grid_y
.
push_back
(
start_y
);
}
}
else
if
(
std
::
abs
(
y
-
last_y
)
<
1e-6
)
{
// current trace is horizontal
int
start_x
=
static_cast
<
int
>
(
std
::
min
(
x
,
last_x
));
int
end_x
=
static_cast
<
int
>
(
std
::
max
(
x
,
last_x
));
int
start_y
=
static_cast
<
int
>
(
y
);
while
((
++
start_x
)
<
end_x
)
{
grid_x
.
push_back
(
start_x
);
grid_y
.
push_back
(
start_y
);
}
}
else
{
double
k
=
Slope
(
last_x
,
last_y
,
x
,
y
);
if
(
k
>
99999999.0
)
{
AERROR
<<
"slope error"
;
return
-
1
;
}
int
steps
=
static_cast
<
int
>
(
std
::
abs
(
last_x
-
x
));
int
step
=
0
;
double
xx
=
0.0
,
yy
=
0.0
;
if
(
x
<
last_x
)
{
xx
=
x
,
yy
=
y
;
}
else
{
xx
=
last_x
,
yy
=
last_y
;
}
while
((
step
++
)
<
steps
)
{
xx
=
xx
+
1
;
yy
=
yy
+
k
;
grid_x
.
push_back
(
static_cast
<
int
>
(
xx
));
grid_y
.
push_back
(
static_cast
<
int
>
(
yy
));
}
}
return
0
;
}
double
LapsChecker
::
Slope
(
double
x1
,
double
y1
,
double
x2
,
double
y2
)
{
if
(
std
::
abs
(
x1
-
x2
)
<
1e-6
)
{
return
std
::
numeric_limits
<
double
>::
max
();
}
if
(
std
::
abs
(
y1
-
y2
)
<
1e-6
)
{
return
0.0
;
}
return
(
y2
-
y1
)
/
(
x2
-
x1
);
}
ErrorCode
LapsChecker
::
GetReturnState
()
{
return
return_state_
;
}
}
// namespace hdmap
}
// namespace apollo
modules/map/tools/map_datachecker/server/laps_checker.h
0 → 100644
浏览文件 @
dd765961
/******************************************************************************
* Copyright 2019 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once
#include <map>
#include <memory>
#include <utility>
#include <vector>
#include "modules/map/tools/map_datachecker/proto/collection_error_code.pb.h"
#include "modules/map/tools/map_datachecker/server/common.h"
namespace
apollo
{
namespace
hdmap
{
struct
GridMeta
{
double
alpha
;
std
::
vector
<
int
>
idxs
;
};
typedef
std
::
vector
<
GridMeta
>
Grid
;
class
LapsChecker
{
public:
explicit
LapsChecker
(
const
std
::
vector
<
FramePose
>&
poses
,
int
laps_to_check
,
std
::
shared_ptr
<
JSonConf
>
sp_conf
);
ErrorCode
Check
();
double
GetProgress
()
const
;
double
GetConfidence
();
size_t
GetLap
()
const
;
ErrorCode
GetReturnState
();
private:
void
DoCheck
();
int
SetupGridsMap
();
int
CheckLaps
();
int
CheckParams
();
int
GetMinMax
();
int
DoSetupGridsMap
();
double
CalcAlpha
(
int
pose_index
);
int
PutPoseToGrid
(
int
pose_index
,
int
grid_y
,
int
grid_x
);
int
PutPoseToNeighborGrid
(
int
pose_index
);
int
GetPassedGrid
(
int
pose_index
,
std
::
vector
<
int
>*
sp_grid_x
,
std
::
vector
<
int
>*
sp_grid_y
);
double
Slope
(
double
x1
,
double
y1
,
double
x2
,
double
y2
);
int
GatherTimestamps
(
std
::
vector
<
double
>*
sp_stamps
,
double
alpha
,
int
center_x
,
int
center_y
);
inline
int
SetProgress
(
double
p
);
public:
const
std
::
vector
<
FramePose
>&
poses_
;
double
maxx_
,
maxy_
,
minx_
,
miny_
;
std
::
vector
<
std
::
vector
<
Grid
>>
grids_map_
;
bool
finished_
;
private:
std
::
vector
<
double
>
confidence_
;
double
progress_
;
size_t
laps_to_check_
;
size_t
possible_max_laps_
;
size_t
lap_
;
std
::
shared_ptr
<
JSonConf
>
sp_conf_
;
ErrorCode
return_state_
;
};
}
// namespace hdmap
}
// namespace apollo
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录