Skip to content
Projects
Groups
Snippets
Help
Loading...
Sign in / Register
Toggle navigation
M
map_location
Project
Project
Details
Activity
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Board
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Packages
Packages
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
oscar
map_location
Commits
20d6377e
Commit
20d6377e
authored
Apr 11, 2022
by
oscar
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
提交地图接口代码
parent
2e6fd105
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
664 additions
and
0 deletions
+664
-0
MapLocation.cpp
src/MapLocation.cpp
+654
-0
MapLocation.h
src/MapLocation.h
+10
-0
No files found.
src/MapLocation.cpp
View file @
20d6377e
#include "MapLocation.h"
#include "MapLocation.h"
#include <sys/time.h>
#include <unistd.h>
namespace
JfxLocation
namespace
JfxLocation
{
{
static
const
double
sc_min_prob
=
0.8
;
uint64_t
GetCurTime
()
{
struct
timeval
time
;
gettimeofday
(
&
time
,
NULL
);
uint64_t
seconds
=
time
.
tv_sec
;
uint64_t
ttt
=
seconds
*
1000
*
1000
+
time
.
tv_usec
;
return
ttt
;
}
MapLocation
::
MapLocation
(
const
std
::
string
&
project
,
const
std
::
string
&
config
)
MapLocation
::
MapLocation
(
const
std
::
string
&
project
,
const
std
::
string
&
config
)
{
{
jf
::
HdLib
::
GetInstance
()
->
Init
(
project
,
config
);
//初始化地图
jf
::
HdLib
::
GetInstance
()
->
Init
(
project
,
config
);
//初始化地图
}
}
MapLocation
::~
MapLocation
()
{
if
(
m_isRun
==
true
)
{
m_isRun
=
false
;
if
(
m_thread
.
joinable
())
{
m_thread
.
join
();
}
}
}
int
MapLocation
::
SetMatchingResultCallback
(
MatchingResultCallback
cb
)
int
MapLocation
::
SetMatchingResultCallback
(
MatchingResultCallback
cb
)
{
{
m_mmMatch
.
Init
();
m_mmMatch
.
Init
();
m_isRun
=
true
;
m_thread
=
std
::
thread
(
&
MapLocation
::
ThreadProcess
,
this
);
std
::
lock_guard
<
std
::
mutex
>
lock
(
m_mtx
);
m_cb
=
cb
;
return
0
;
return
0
;
}
}
void
MapLocation
::
ThreadProcess
()
{
uint64_t
tip_time
=
GetCurTime
();
while
(
m_isRun
)
{
if
(
GetCurTime
()
-
tip_time
>
100
*
1000
)
{
IMatchResult
data
=
{};
if
(
true
==
m_mmMatch
.
GetMatchResultData
(
data
))
{
MatchingResult
result
=
{};
result
.
time_stamp
=
(
int64_t
)
data
.
timestamp
.
sec
*
1e6
+
(
int64_t
)
data
.
timestamp
.
nanosec
/
1000
;
result
.
foot_point
.
x
=
data
.
foot_point
.
x
;
result
.
foot_point
.
y
=
data
.
foot_point
.
y
;
result
.
foot_point
.
z
=
data
.
foot_point
.
z
;
result
.
foot_prob
=
data
.
foot_prob
;
result
.
vision_horizontal_point
.
x
=
data
.
vision_horizontal_point
.
x
;
result
.
vision_horizontal_point
.
y
=
data
.
vision_horizontal_point
.
y
;
result
.
vision_horizontal_point
.
z
=
data
.
vision_horizontal_point
.
z
;
result
.
horizontal_prob
=
data
.
horizontal_prob
;
result
.
vision_vertical_point
.
x
=
data
.
vision_vertical_point
.
x
;
result
.
vision_vertical_point
.
y
=
data
.
vision_vertical_point
.
y
;
result
.
vision_vertical_point
.
z
=
data
.
vision_vertical_point
.
z
;
result
.
vertical_prob
=
data
.
vertical_prob
;
result
.
match_status
=
data
.
match_status
;
result
.
change_lanes
=
(
int8_t
)
data
.
change_lanes
;
result
.
lane_seq_no
=
data
.
lane_seq_no
;
result
.
lane_num
=
data
.
lane_num
;
result
.
road_rank
=
data
.
road_rank
;
result
.
road_type
=
data
.
road_type
;
result
.
angle_with_lane
=
data
.
angle_with_lane
;
result
.
special_type
=
data
.
special_type
;
result
.
special_type_begin_dis
=
data
.
special_type_begin_dis
;
result
.
special_type_end_dis
=
data
.
special_type_end_dis
;
result
.
in_road_crossing
=
data
.
in_road_crossing
;
result
.
left_lane_edge_dis
=
data
.
left_lane_edge_dis
;
result
.
right_lane_edge_dis
=
data
.
right_lane_edge_dis
;
result
.
gnss_time
=
data
.
gnss_time
;
result
.
gnss_longitude
=
data
.
gnss_point
.
x
;
result
.
gnss_latitude
=
data
.
gnss_point
.
y
;
result
.
gnss_height
=
data
.
gnss_point
.
z
;
result
.
gnss_heading
=
data
.
gnss_heading
;
result
.
gnss_ground_speed
=
data
.
gnss_speed
;
result
.
gnss_longitude_standard_deviation
=
data
.
gnss_longitude_deviation
;
result
.
gnss_latitude_standard_deviation
=
data
.
gnss_latitude_deviation
;
result
.
gnss_height_standard_deviation
=
data
.
gnss_height_deviation
;
result
.
gnss_solution_status
=
data
.
gnss_solution_status
;
std
::
lock_guard
<
std
::
mutex
>
lock
(
m_mtx
);
if
(
m_cb
)
m_cb
(
result
);
}
tip_time
+=
100
*
1000
;
}
else
{
int64_t
sleep
=
tip_time
+
100
*
1000
-
GetCurTime
();
if
(
sleep
>
0
)
usleep
(
sleep
);
}
}
}
int
MapLocation
::
PushGPSFrame
(
GPSFrame
&
gps
)
int
MapLocation
::
PushGPSFrame
(
GPSFrame
&
gps
)
{
{
IGnssLocationData
gnss_location_data
=
{};
gnss_location_data
.
timestamp
.
sec
=
gps
.
time_stamp
/
1e6
;
gnss_location_data
.
timestamp
.
nanosec
=
(
gps
.
time_stamp
%
1e6
)
*
1000
;
gnss_location_data
.
gnss_point
.
x
=
gps
.
longitude
;
gnss_location_data
.
gnss_point
.
y
=
gps
.
latitude
;
gnss_location_data
.
gnss_point
.
z
=
gps
.
altitude
;
gnss_location_data
.
heading
=
gps
.
heading
;
gnss_location_data
.
speed
=
gps
.
ground_speed
;
gnss_location_data
.
coefficient
=
gps
.
pitch_deviation
;
if
(
sqrt
(
pow
(
gps
.
longitude_deviation
,
2
)
+
pow
(
gps
.
latitude_deviation
,
2
))
<=
0.01
)
{
gnss_location_data
.
credible
=
true
;
}
else
{
gnss_location_data
.
credible
=
false
;
}
gnss_location_data
.
credible
=
true
;
// gnss_location_data.gnss_time = 315936000.0 - 18 + msg->gps_time_.gnss_week_ * 7 * 86400 + msg->gps_time_.seconds_from_week_start_ + 8 * 3600;
gnss_location_data
.
gnss_time
=
gps
.
jfx_gps_time
;
gnss_location_data
.
longitude_deviation
=
gps
.
longitude_deviation_
;
gnss_location_data
.
latitude_deviation
=
gps
.
latitude_deviation
;
gnss_location_data
.
height_deviation
=
gps
.
height_deviation
;
gnss_location_data
.
solution_status
=
gps
.
solution_status
;
m_mmMatch
.
SaveGnssLocData
(
gnss_location_data
);
return
0
;
return
0
;
}
}
bool
ConvertLanePos
(
int
src
,
LaneMarkingSide
&
dst
)
{
switch
(
src
)
{
case
1
:
//
dst
=
LaneMarkingSide
::
LANE_MARKING_SIDE_LEFT
;
return
true
;
case
2
:
// right
dst
=
LaneMarkingSide
::
LANE_MARKING_SIDE_RIGHT
;
return
true
;
case
3
:
//
dst
=
LaneMarkingSide
::
LANE_MARKING_SIDE_LEFT_LEFT
;
return
true
;
case
4
:
//
dst
=
LaneMarkingSide
::
LANE_MARKING_SIDE_RIGHT_RIGHT
;
return
true
;
default
:
return
false
;
}
return
false
;
}
bool
ConvertLaneColor
(
int
src
,
HMC
::
LaneMarkingColor
&
dst
)
{
switch
(
src
)
{
case
1
:
dst
=
HMC
::
LaneMarkingColor
::
HDM_LANE_MARKING_COLOR_WHITE
;
return
true
;
case
2
:
//
dst
=
HMC
::
LaneMarkingColor
::
HDM_LANE_MARKING_COLOR_YELLOW
;
return
true
;
case
3
:
//
dst
=
HMC
::
LaneMarkingColor
::
HDM_LANE_MARKING_COLOR_BLUE
;
return
true
;
default
:
dst
=
HMC
::
LaneMarkingColor
::
HDM_LANE_MARKING_COLOR_WHITE
;
return
false
;
};
return
false
;
}
bool
ConvertLaneCross
(
int
src_double
,
int
src_single
,
HMC
::
LaneMarkingType
&
dst
)
{
if
(
0
!=
src_double
)
{
switch
(
src_double
)
{
case
1
:
//
dst
=
HMC
::
LaneMarkingType
::
HDM_LANE_MARKING_TYPE_LEFT_SOLID_RIGHT_DASHED
;
return
true
;
case
2
:
//
dst
=
HMC
::
LaneMarkingType
::
HDM_LANE_MARKING_TYPE_RIGHT_SOLID_LEFT_DASHED
;
return
true
;
case
3
:
//
dst
=
HMC
::
LaneMarkingType
::
HDM_LANE_MARKING_TYPE_DOUBLE_SOLID
;
return
true
;
case
4
:
//
dst
=
HMC
::
LaneMarkingType
::
HDM_LANE_MARKING_TYPE_DOUBLE_DASHED
;
return
true
;
default
:
return
false
;
};
}
else
{
switch
(
src_single
)
{
case
1
:
//
dst
=
HMC
::
LaneMarkingType
::
HDM_LANE_MARKING_TYPE_SINGLE_SOLID
;
return
true
;
case
2
:
//
dst
=
HMC
::
LaneMarkingType
::
HDM_LANE_MARKING_TYPE_SINGLE_DASHED
;
return
true
;
case
3
:
//
return
false
;
case
4
:
//
dst
=
HMC
::
LaneMarkingType
::
HDM_LANE_MARKING_TYPE_SHORT_THICK_DASHED
;
return
true
;
default
:
return
false
;
}
}
return
false
;
}
bool
ConvertLinkPos
(
int
src
,
LinkMarkingSide
&
dst
)
{
switch
(
src
)
{
case
1
:
//
case
3
:
//
dst
=
LinkMarkingSide
::
LINK_MARKING_SIDE_LEFT
;
return
true
;
case
2
:
//
case
4
:
//
dst
=
LinkMarkingSide
::
LINK_MARKING_SIDE_RIGHT
;
return
true
;
default
:
return
false
;
}
return
false
;
}
bool
ConvertTsr
(
const
Lines
&
src
,
ITrafsignMap
&
dst
)
{
switch
(
src
.
tsr_sign_name
)
{
case
0
:
// Regular 10
case
28
:
// Electronic 10
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
10
;
break
;
case
1
:
// Regular 20
case
29
:
// Electronic 20
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
20
;
break
;
case
2
:
// Regular 30
case
30
:
// Electronic 30
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
30
;
break
;
case
3
:
// Regular 40
case
31
:
// Electronic 40
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
40
;
break
;
case
4
:
// Regular 50
case
32
:
// Electronic 50
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
50
;
break
;
case
5
:
// Regular 60
case
33
:
// Electronic 60
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
60
;
break
;
case
6
:
// Regular 70
case
34
:
// Electronic 70
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
70
;
break
;
case
7
:
// Regular 80
case
35
:
// Electronic 80
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
80
;
break
;
case
8
:
// Regular 90
case
36
:
// Electronic 90
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
90
;
break
;
case
9
:
// Regular 100
case
37
:
// Electronic 100
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
100
;
break
;
case
10
:
// Regular 110
case
38
:
// Electronic 110
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
110
;
break
;
case
11
:
// Regular 120
case
39
:
// Electronic 120
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
120
;
break
;
case
12
:
// Regular 130
case
40
:
// Electronic 130
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
130
;
break
;
case
41
:
// Electronic 140
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
140
;
case
23
:
// Car Limit
break
;
case
25
:
// Minimum Speed Limits
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
14
;
break
;
case
26
:
// Motorcycle Limit
break
;
case
79
:
// End of Speed 2 digits
break
;
case
80
:
// End of Speed 3 digits
break
;
case
100
:
// Regular 5
case
115
:
// Electronic 5
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
5
;
break
;
case
101
:
// Regular 15
case
116
:
// Electronic 15
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
15
;
break
;
case
102
:
// Regular 25
case
117
:
// Electronic 25
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
25
;
break
;
case
103
:
// Regular 35
case
118
:
// Electronic 35
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
35
;
break
;
case
104
:
// Regular 45
case
119
:
// Electronic 45
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
45
;
break
;
case
105
:
// Regular 55
case
120
:
// Electronic 55
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
55
;
break
;
case
106
:
// Regular 65
case
121
:
// Electronic 65
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
65
;
break
;
case
107
:
// Regular 75
case
122
:
// Electronic 75
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
75
;
break
;
case
108
:
// Regular 85
case
123
:
// Electronic 85
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
85
;
break
;
case
109
:
// Regular 95
case
124
:
// Electronic 95
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
95
;
break
;
case
110
:
// Regular 105
case
125
:
// Electronic 105
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
105
;
break
;
case
111
:
// Regular 115
case
126
:
// Electronic 115
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
115
;
break
;
case
112
:
// Regular 125
case
127
:
// Electronic 125
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
125
;
break
;
case
113
:
// Regular 135
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
135
;
break
;
case
114
:
// Regular 145
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
11
;
dst
.
speed_limit_value
=
145
;
break
;
case
152
:
// Right Merge
dst
.
traffic_sign_class
=
1
;
dst
.
traffic_sign_sub_class
=
49
;
break
;
case
207
:
// No stopping
break
;
case
256
:
// Bicycle Lane
break
;
case
260
:
// Height Limit
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
8
;
break
;
case
266
:
// Weight Limit
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
9
;
break
;
case
267
:
// Width Limit
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
13
;
break
;
default
:
break
;
}
switch
(
src
.
tsr_sup1_sign_name
)
{
case
0
:
// None
break
;
case
1
:
// Rain
break
;
case
2
:
// Snow
break
;
case
3
:
// Trailer
break
;
case
4
:
// Time
break
;
case
5
:
// Arrow_left
dst
.
traffic_sign_class
=
4
;
dst
.
traffic_sign_sub_class
=
3
;
case
6
:
// Arrow_right
dst
.
traffic_sign_class
=
4
;
dst
.
traffic_sign_sub_class
=
4
;
case
7
:
// BendArrow_left
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
5
;
case
8
:
// BendArrow_right
dst
.
traffic_sign_class
=
5
;
dst
.
traffic_sign_sub_class
=
6
;
case
9
:
// Truck
break
;
case
10
:
// Distance_arrow(distance_for)
break
;
case
11
:
// Weight
break
;
case
12
:
// Distance_in
break
;
case
13
:
// Tractor
break
;
case
14
:
// Snow_rain
break
;
case
15
:
// School
dst
.
traffic_sign_class
=
6
;
break
;
case
16
:
// Rain_cloud
break
;
case
17
:
// Fog
break
;
case
18
:
// Hazerdous_materials
break
;
case
19
:
// Night
break
;
case
20
:
// Supp_sign_generic
break
;
case
21
:
// e_rappel
break
;
case
22
:
// e_zone
break
;
case
23
:
// Ramp
dst
.
traffic_sign_class
=
2
;
dst
.
traffic_sign_sub_class
=
9
;
dst
.
traffic_sign_sub_class
=
10
;
break
;
case
24
:
// End
break
;
case
25
:
// Exit
break
;
case
26
:
// Reserved
break
;
case
27
:
// Minimum
break
;
case
34
:
// Arrow_bidirectional
break
;
case
35
:
// Work_Zone
break
;
case
255
:
//= Invalid_supp;
break
;
default
:
break
;
}
return
true
;
}
int
MapLocation
::
PushLines
(
Lines
&
lines
)
int
MapLocation
::
PushLines
(
Lines
&
lines
)
{
{
/* 我们需要的数据
lanes_mark_count_ 车道线个数
lanes_mark_info_ 单个车道线信息
line_exist_prob_ 置信度
line_color_ 车道线颜色(最好有,没有也行)
line_dlm_type_ 车道线双线的类型
line_lane_mark_type_ 车道线单线的类型
line_c0_ 车离车道线距离
line_c1_ 车的方向与车道线的角度
line_side_ 车道线是左1,左2,右1,右2 中的一种
line_type_ 车道线是车道边界还是道路边界
lane_info_ 车道信息
left_crossing_ 是否是向左跨线
right_crossing_ 是否向右跨线
view_start_ 离车道线起始点的距离(单位:)
view_end_ 离车道线结束点的距离(单位:)
tsr_count_ 标牌数量
tsr_info_ 标牌信息
*/
IVisionMap
vision
=
{};
// time stamp
vision
.
timestamp
.
sec
=
lines
.
time_stamp
/
1e6
;;
vision
.
timestamp
.
nanosec
=
(
lines
.
time_stamp
%
1e6
)
*
1000
;
// crossing
if
(
true
==
lines
.
left_crossing
)
{
vision
.
crossing
=
MarkingCrossing
::
EDGE_CROSSING_LEFT_CROSSING
;
// ROS_WARN("RosInterface::ReceiveAduData(), left crossing");
}
else
if
(
true
==
lines
.
right_crossing
)
{
vision
.
crossing
=
MarkingCrossing
::
EDGE_CROSSING_RIGHT_CROSSING
;
// ROS_WARN("RosInterface::ReceiveAduData(), right crossing");
}
else
{
vision
.
crossing
=
MarkingCrossing
::
EDGE_CROSSING_NO_CROSSING
;
}
// lane marks
for
(
int
i
=
0
;
i
<
lines
.
lines
.
size
();
i
++
)
{
Line
&
lane
=
lines
.
lines
[
i
];
if
(
0
==
lane
.
line_type
)
{
//ROS_WARN("RosInterface::ReceiveAduData(), invalid type: %d", lane.line_type_);
continue
;
}
// lane edge
else
if
(
1
==
lane
.
line_type
)
{
if
(
lane
.
conf
>=
sc_min_prob
&&
lane
.
line_c0
!=
0
)
{
ILaneMarking
lane_edge
=
{};
if
(
false
==
ConvertLanePos
(
lane
.
line_side
,
lane_edge
.
pos
))
{
ROS_WARN
(
"RosInterface::ReceiveAduData(), invalid side type: %d"
,
lane
.
line_side_
);
continue
;
}
if
(
false
==
ConvertLaneColor
(
lane
.
line_color
,
lane_edge
.
lane_marking_color
))
{
ROS_WARN
(
"RosInterface::ReceiveAduData(), invalid color type: %d"
,
lane
.
line_color_
);
continue
;
}
// lane_edge.color_prob = lane.line_color_prob_;
if
(
false
==
ConvertLaneCross
(
lane
.
line_dlm_type
,
lane
.
line_lane_mark_type
,
lane_edge
.
cross_type
))
{
ROS_WARN
(
"RosInterface::ReceiveAduData(), invalid cross type: dlm = %d, mark = %d"
,
lane
.
line_dlm_type_
,
lane
.
line_lane_mark_type_
);
continue
;
}
lane_edge
.
angle
=
lane
.
line_c1
;
lane_edge
.
dis
=
lane
.
line_c0
;
lane_edge
.
exist_prob
=
lane
.
conf
*
100
;
vision
.
lane_markings
.
push_back
(
lane_edge
);
}
}
// road edge
else
if
(
2
==
lane
.
line_type
)
{
if
(
lane
.
conf
>=
sc_min_prob
&&
lane
.
line_c0
!=
0
)
{
ILinkMarking
link_edge
=
{};
if
(
false
==
ConvertLinkPos
(
lane
.
line_side
,
link_edge
.
pos
))
{
continue
;
}
if
(
false
==
ConvertLaneCross
(
lane
.
line_dlm_type
,
lane
.
line_lane_mark_type
,
link_edge
.
type
))
{
continue
;
}
link_edge
.
angle
=
lane
.
line_c1
;
link_edge
.
dis
=
lane
.
line_c0
;
link_edge
.
exist_prob
=
lane
.
conf
;
if
(
LinkMarkingSide
::
LINK_MARKING_SIDE_LEFT
==
link_edge
.
pos
)
{
vision
.
left_link_marking
.
push_back
(
link_edge
);
}
else
{
vision
.
right_link_marking
.
push_back
(
link_edge
);
}
// ROS_WARN("RosInterface::ReceiveAduData(), time: %d seconds, link pos = %d, lane1 prob = %d:%d", msg->header.stamp - start_time, link_edge.pos, lane.line_color_prob_, lane.line_exist_prob_);
}
}
// invalid type
else
{
//ROS_WARN("RosInterface::ReceiveAduData(), invalid type: %d", lane.line_type_);
continue
;
}
}
if
(
true
==
vision
.
lane_markings
.
empty
())
{
//ROS_WARN("RosInterface::ReceiveAduData(), no valid lane");
return
0
;
}
if
(
0
!=
lines
.
tsr_shape
)
{
ITrafsignMap
trafsign
=
{};
trafsign
.
lat_dis
=
lines
.
tsr_sign_lat_dis
;
trafsign
.
lon_dis
=
lines
.
tsr_sign_long_dis
;
trafsign
.
traffic_sign_shape
=
lines
.
tsr_shape
;
trafsign
.
exist_prob
=
lines
.
tsr_prob
;
vision
.
traffic_signs
.
push_back
(
trafsign
);
ConvertTsr
(
lines
,
trafsign
);
// ROS_WARN("RosInterface::ReceiveAduData(), time: %d seconds, shape = %d, lat dis = %f, long dis = %f, tsr prob = %d", msg->header.stamp - start_time, msg->tsr_info_.tsr_shape_, msg->tsr_info_.tsr_sign_lat_dis_,
// msg->tsr_info_.tsr_sign_long_dis_,
// msg->tsr_info_.tsr_prob_);
}
static
int
s_nRcvCnt
=
0
;
if
(
s_nRcvCnt
%
4
==
0
)
{
m_mmMatch
.
SaveVisionMapData
(
vision
);
}
s_nRcvCnt
++
;
return
0
;
return
0
;
}
}
int
MapLocation
::
PushLocation
(
Location
&
loc
)
int
MapLocation
::
PushLocation
(
Location
&
loc
)
{
{
auto
func_wraps2pi_pi
=
[](
double
&
rad
)
{
while
(
rad
>
pi
)
rad
-=
2
*
pi
;
while
(
rad
<
(
-
pi
))
rad
+=
2
*
pi
;
return
rad
;
};
auto
func_Quat2Euler
=
[
&
func_wraps2pi_pi
](
double
q0
,
double
q1
,
double
q2
,
double
q3
,
double
&
roll
,
double
&
pitch
,
double
&
heading
)
{
double
gamma
=
atan2
(
2.0
*
(
q0
*
q1
+
q2
*
q3
),
1.0
-
2.0
*
(
q1
*
q1
+
q2
*
q2
));
double
theta
=
asin
(
2.0
*
(
q0
*
q2
-
q3
*
q1
));
double
psi
=
atan2
(
2.0
*
(
q0
*
q3
+
q1
*
q2
),
1.0
-
2.0
*
(
q2
*
q2
+
q3
*
q3
));
roll
=
func_wraps2pi_pi
(
gamma
);
pitch
=
func_wraps2pi_pi
(
theta
);
heading
=
func_wraps2pi_pi
(
psi
);
};
ILocalization
loc_
=
{};
loc_
.
lTimestamp
=
(
double
)
loc
.
time_stamp
/
1e6
;
loc_
.
loc_point
.
x
=
loc
.
pose
.
position
.
x
;
loc_
.
loc_point
.
y
=
loc
.
pose
.
position
.
y
;
loc_
.
loc_point
.
z
=
loc
.
pose
.
position
.
z
;
loc_
.
gnss_available
=
loc
.
gnss_available
;
func_Quat2Euler
(
loc
.
pose
.
orientation
.
x
,
loc
.
pose
.
orientation
.
y
,
loc
.
pose
.
orientation
.
z
,
loc
.
pose
.
orientation
.
w
,
loc_
.
roll
,
loc_
.
pitch
,
loc_
.
heading
);
loc_
.
heading
=
loc
.
attitude
.
yaw
<
0
?
loc
.
attitude
.
yaw
+
360
:
loc
.
attitude
.
yaw
;
loc_
.
speed
=
pow
((
pow
(
loc
.
velocity
.
linear_velocity
.
x
,
2
)
+
pow
(
loc
.
velocity
.
angular_velocity
.
y
,
2
)),
0.5
);
loc_
.
longitude_accuracy
=
loc
.
acceleration
.
std_linear_acceleration
.
x
;
loc_
.
latitude_accuracy
=
loc
.
acceleration
.
std_linear_acceleration
.
y
;
// SendLocRes(msg, loc.speed);
// PublishLocCanFd(msg, loc.speed);
static
int
s_nRcvCnt2
=
0
;
if
(
s_nRcvCnt2
%
10
==
0
)
{
m_mmMatch
.
SaveEkfResultData
(
loc_
);
}
s_nRcvCnt2
++
;
return
0
;
return
0
;
}
}
}
}
src/MapLocation.h
View file @
20d6377e
...
@@ -3,6 +3,8 @@
...
@@ -3,6 +3,8 @@
#include "MapLocationBase.h"
#include "MapLocationBase.h"
#include "HdLib.h"
#include "HdLib.h"
#include "MapMatching.hpp"
#include "MapMatching.hpp"
#include <thread>
#include <mutex>
namespace
JfxLocation
namespace
JfxLocation
{
{
...
@@ -10,6 +12,7 @@ namespace JfxLocation
...
@@ -10,6 +12,7 @@ namespace JfxLocation
{
{
public
:
public
:
MapLocation
(
const
std
::
string
&
project
,
const
std
::
string
&
config
);
MapLocation
(
const
std
::
string
&
project
,
const
std
::
string
&
config
);
~
MapLocation
();
virtual
int
SetMatchingResultCallback
(
MatchingResultCallback
cb
);
virtual
int
SetMatchingResultCallback
(
MatchingResultCallback
cb
);
virtual
int
PushGPSFrame
(
GPSFrame
&
gps
);
virtual
int
PushGPSFrame
(
GPSFrame
&
gps
);
...
@@ -17,8 +20,15 @@ namespace JfxLocation
...
@@ -17,8 +20,15 @@ namespace JfxLocation
virtual
int
PushLocation
(
Location
&
loc
);
virtual
int
PushLocation
(
Location
&
loc
);
public
:
public
:
void
ThreadProcess
();
jf
::
MapMatching
m_mmMatch
;
jf
::
MapMatching
m_mmMatch
;
bool
m_isRun
=
false
;
//记录是否在运行
std
::
thread
m_thread
;
//运行线程
std
::
mutex
m_mtx
;
MatchingResultCallback
m_cb
=
nullptr
;
};
};
}
}
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a 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