Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
mobile_robotics
autonomous_driving
autonomous_driving_tools
Commits
7534e38d
Commit
7534e38d
authored
Jan 27, 2021
by
Sergi Hernandez
Browse files
Added a function to get the previous segment.
Made some functions const.
parent
51fec26a
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/opendrive_road_segment.h
View file @
7534e38d
...
...
@@ -63,17 +63,15 @@ class COpendriveRoadSegment
void
add_node
(
TOpendriveRoadSegmentGeometry
&
geometry
,
COpendriveLane
*
lane
);
void
add_empty_node
(
TOpendriveWorldPose
&
pose
,
COpendriveLane
*
lane
);
void
remove_node
(
COpendriveRoadNode
*
node
);
bool
has_node
(
COpendriveRoadNode
*
node
);
int
get_node_side
(
COpendriveRoadNode
*
node
);
bool
has_node
(
COpendriveRoadNode
*
node
)
const
;
void
link_neightbour_lanes
(
lanes
::
laneSection_type
&
lane_section
);
void
link_neightbour_lanes
(
void
);
void
link_segment
(
COpendriveRoadSegment
*
segment
);
void
link_segment
(
COpendriveRoadSegment
*
segment
,
int
from
,
bool
from_start
,
int
to
,
bool
to_start
);
bool
connects_to
(
COpendriveRoadSegment
*
segment
);
bool
connects_segments
(
COpendriveRoadSegment
*
segment1
,
COpendriveRoadSegment
*
segment2
);
COpendriveRoadSegment
*
get_sub_segment
(
node_up_ref_t
&
new_node_ref
,
lane_up_ref_t
&
new_lane_ref
,
link_up_ref_t
&
new_link_ref
,
int
node_side
,
TOpendriveWorldPose
*
start
=
NULL
,
TOpendriveWorldPose
*
end
=
NULL
);
COpendriveRoadSegment
*
clone
(
node_up_ref_t
&
new_node_ref
,
lane_up_ref_t
&
new_lane_ref
,
link_up_ref_t
&
new_link_ref
);
COpendriveRoadSegment
*
get_next_segment
(
int
&
side
);
COpendriveRoadSegment
*
get_sub_segment
(
node_up_ref_t
&
new_node_ref
,
lane_up_ref_t
&
new_lane_ref
,
link_up_ref_t
&
new_link_ref
,
int
node_side
,
TOpendriveWorldPose
*
start
=
NULL
,
TOpendriveWorldPose
*
end
=
NULL
)
const
;
COpendriveRoadSegment
*
clone
(
node_up_ref_t
&
new_node_ref
,
lane_up_ref_t
&
new_lane_ref
,
link_up_ref_t
&
new_link_ref
)
const
;
public:
std
::
string
get_name
(
void
)
const
;
unsigned
int
get_id
(
void
)
const
;
...
...
@@ -89,16 +87,19 @@ class COpendriveRoadSegment
const
COpendriveObject
&
get_object
(
unsigned
int
index
)
const
;
unsigned
int
get_num_connecting
(
void
)
const
;
const
COpendriveRoadSegment
&
get_connecting
(
unsigned
int
index
)
const
;
double
get_length
(
void
);
TOpendriveWorldPose
get_pose_at
(
double
length
);
double
get_curvature_at
(
double
length
);
double
get_length
(
void
)
const
;
TOpendriveWorldPose
get_pose_at
(
double
length
)
const
;
double
get_curvature_at
(
double
length
)
const
;
TOpendriveLocalPose
transform_to_local_pose
(
const
TOpendriveTrackPose
&
track
);
TOpendriveWorldPose
transform_to_world_pose
(
const
TOpendriveTrackPose
&
track
);
const
COpendriveRoadNode
&
get_closest_node
(
TOpendriveWorldPose
&
pose
,
double
&
distance
,
double
angle_tol
=
0.1
)
const
;
unsigned
int
get_closest_node_index
(
TOpendriveWorldPose
&
pose
,
double
&
distance
,
double
angle_tol
=
0.1
)
const
;
const
COpendriveLane
&
get_closest_lane
(
TOpendriveWorldPose
&
pose
,
double
&
distance
,
double
angle_tol
=
0.1
);
const
COpendriveLane
&
get_closest_lane
(
TOpendriveWorldPose
&
pose
,
double
&
distance
,
double
angle_tol
=
0.1
)
const
;
int
get_closest_lane_id
(
TOpendriveWorldPose
&
pose
,
double
&
distance
,
double
angle_tol
=
0.1
)
const
;
double
get_closest_pose
(
TOpendriveWorldPose
&
pose
,
TOpendriveWorldPose
&
closest_pose
,
double
angle_tol
=
0.1
)
const
;
const
COpendriveRoadSegment
&
get_next_segment
(
int
&
side
,
bool
&
valid
)
const
;
const
COpendriveRoadSegment
&
get_prev_segment
(
int
&
side
,
bool
&
valid
)
const
;
int
get_pose_side
(
TOpendriveWorldPose
&
pose
)
const
;
friend
std
::
ostream
&
operator
<<
(
std
::
ostream
&
out
,
COpendriveRoadSegment
&
segment
);
~
COpendriveRoadSegment
();
};
...
...
src/opendrive_road_segment.cpp
View file @
7534e38d
...
...
@@ -521,7 +521,7 @@ void COpendriveRoadSegment::remove_node(COpendriveRoadNode *node)
}
}
bool
COpendriveRoadSegment
::
has_node
(
COpendriveRoadNode
*
node
)
bool
COpendriveRoadSegment
::
has_node
(
COpendriveRoadNode
*
node
)
const
{
for
(
unsigned
int
i
=
0
;
i
<
this
->
nodes
.
size
();
i
++
)
if
(
this
->
nodes
[
i
]
==
node
)
...
...
@@ -530,25 +530,20 @@ bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node)
return
false
;
}
int
COpendriveRoadSegment
::
get_
nod
e_side
(
C
Opendrive
RoadNode
*
node
)
int
COpendriveRoadSegment
::
get_
pos
e_side
(
T
Opendrive
WorldPose
&
pose
)
const
{
std
::
map
<
int
,
COpendriveLane
*>::
iterator
it
;
const
COpendriveRoadNode
*
node
;
std
::
map
<
int
,
COpendriveLane
*>::
const_iterator
it
;
std
::
stringstream
error
;
double
distance
;
if
(
this
->
has_node
(
node
))
{
for
(
it
=
this
->
lanes
.
begin
();
it
!=
this
->
lanes
.
end
();
it
++
)
if
(
it
->
second
->
has_node
(
node
))
return
it
->
first
;
node
=&
this
->
get_closest_node
(
pose
,
distance
,
3.14159
);
for
(
it
=
this
->
lanes
.
begin
();
it
!=
this
->
lanes
.
end
();
it
++
)
if
(
it
->
second
->
has_node
((
COpendriveRoadNode
*
)
node
))
return
it
->
first
;
error
<<
"Segment "
<<
this
->
name
<<
" does not include node "
<<
node
->
get_index
();
throw
CException
(
_HERE_
,
error
.
str
());
}
else
{
error
<<
"Segment "
<<
this
->
name
<<
" does not include node "
<<
node
->
get_index
();
throw
CException
(
_HERE_
,
error
.
str
());
}
error
<<
"Segment "
<<
this
->
name
<<
" does not include node "
<<
node
->
get_index
();
throw
CException
(
_HERE_
,
error
.
str
());
}
void
COpendriveRoadSegment
::
link_neightbour_lanes
(
lanes
::
laneSection_type
&
lane_section
)
...
...
@@ -690,7 +685,7 @@ bool COpendriveRoadSegment::connects_segments(COpendriveRoadSegment *segment1,CO
return
false
;
}
COpendriveRoadSegment
*
COpendriveRoadSegment
::
get_sub_segment
(
node_up_ref_t
&
new_node_ref
,
lane_up_ref_t
&
new_lane_ref
,
link_up_ref_t
&
new_link_ref
,
int
node_side
,
TOpendriveWorldPose
*
start
,
TOpendriveWorldPose
*
end
)
COpendriveRoadSegment
*
COpendriveRoadSegment
::
get_sub_segment
(
node_up_ref_t
&
new_node_ref
,
lane_up_ref_t
&
new_lane_ref
,
link_up_ref_t
&
new_link_ref
,
int
node_side
,
TOpendriveWorldPose
*
start
,
TOpendriveWorldPose
*
end
)
const
{
TOpendriveWorldPose
*
start_pose
,
*
end_pose
;
std
::
map
<
int
,
COpendriveLane
*>::
iterator
lane_it
;
...
...
@@ -708,7 +703,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
if
(
start
==
NULL
&&
end
==
NULL
)
return
this
->
clone
(
new_node_ref
,
new_lane_ref
,
new_link_ref
);
new_segment
=
new
COpendriveRoadSegment
(
*
this
);
new_segment_ref
[
this
]
=
new_segment
;
new_segment_ref
[
(
COpendriveRoadSegment
*
)
this
]
=
new_segment
;
if
(
node_side
<
0
)
{
start_pose
=
start
;
...
...
@@ -770,7 +765,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
return
new_segment
;
}
COpendriveRoadSegment
*
COpendriveRoadSegment
::
clone
(
node_up_ref_t
&
new_node_ref
,
lane_up_ref_t
&
new_lane_ref
,
link_up_ref_t
&
new_link_ref
)
COpendriveRoadSegment
*
COpendriveRoadSegment
::
clone
(
node_up_ref_t
&
new_node_ref
,
lane_up_ref_t
&
new_lane_ref
,
link_up_ref_t
&
new_link_ref
)
const
{
std
::
map
<
int
,
COpendriveLane
*>::
iterator
lane_it
;
lane_up_ref_t
::
iterator
lane_ref_it
;
...
...
@@ -781,7 +776,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
node_up_ref_t
::
iterator
node_ref_it
;
new_segment
=
new
COpendriveRoadSegment
(
*
this
);
new_segment_ref
[
this
]
=
new_segment
;
new_segment_ref
[
(
COpendriveRoadSegment
*
)
this
]
=
new_segment
;
/* get the sublanes */
for
(
lane_it
=
new_segment
->
lanes
.
begin
();
lane_it
!=
new_segment
->
lanes
.
end
();
lane_it
++
)
{
...
...
@@ -793,7 +788,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
return
new_segment
;
}
COpendriveRoadSegment
*
COpendriveRoadSegment
::
get_next_segment
(
int
&
side
)
const
COpendriveRoadSegment
&
COpendriveRoadSegment
::
get_next_segment
(
int
&
side
,
bool
&
valid
)
const
{
std
::
vector
<
COpendriveRoadSegment
*>
segment_candidates
;
std
::
vector
<
int
>
side_candidates
;
...
...
@@ -848,16 +843,90 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_next_segment(int &side)
}
}
if
(
segment_candidates
.
size
()
==
0
)
return
NULL
;
{
valid
=
false
;
return
*
this
;
}
else
if
(
segment_candidates
.
size
()
>
1
)
throw
CException
(
_HERE_
,
"Road has multiple paths and no unique subroad exists"
);
else
{
valid
=
true
;
side
=
side_candidates
[
0
];
return
*
segment_candidates
[
0
];
}
}
const
COpendriveRoadSegment
&
COpendriveRoadSegment
::
get_prev_segment
(
int
&
side
,
bool
&
valid
)
const
{
std
::
vector
<
COpendriveRoadSegment
*>
segment_candidates
;
std
::
vector
<
int
>
side_candidates
;
bool
already_present
;
if
(
side
<
0
)
{
for
(
unsigned
int
i
=
1
;
i
<=
this
->
get_num_right_lanes
();
i
++
)
{
const
COpendriveLane
&
lane
=
this
->
get_lane
(
-
i
);
if
(
lane
.
get_num_prev_lanes
()
>
1
)
throw
CException
(
_HERE_
,
"Road has multiple paths and no unique subroad exists"
);
if
(
lane
.
get_num_prev_lanes
()
==
1
)
{
already_present
=
false
;
for
(
unsigned
int
k
=
0
;
k
<
segment_candidates
.
size
();
k
++
)
if
(
segment_candidates
[
k
]
==
lane
.
get_prev_lane
(
0
).
segment
)
{
already_present
=
true
;
break
;
}
if
(
!
already_present
)
{
segment_candidates
.
push_back
(
lane
.
get_prev_lane
(
0
).
segment
);
side_candidates
.
push_back
(
lane
.
get_prev_lane
(
0
).
get_id
());
}
}
}
}
else
{
for
(
unsigned
int
i
=
1
;
i
<=
this
->
get_num_left_lanes
();
i
++
)
{
const
COpendriveLane
&
lane
=
this
->
get_lane
(
i
);
if
(
lane
.
get_num_prev_lanes
()
>
1
)
throw
CException
(
_HERE_
,
"Road has multiple paths and no unique subroad exists"
);
if
(
lane
.
get_num_prev_lanes
()
==
1
)
{
already_present
=
false
;
for
(
unsigned
int
k
=
0
;
k
<
segment_candidates
.
size
();
k
++
)
if
(
segment_candidates
[
k
]
==
lane
.
get_prev_lane
(
0
).
segment
)
{
already_present
=
true
;
break
;
}
if
(
!
already_present
)
{
segment_candidates
.
push_back
(
lane
.
get_prev_lane
(
0
).
segment
);
side_candidates
.
push_back
(
lane
.
get_prev_lane
(
0
).
get_id
());
}
}
}
}
if
(
segment_candidates
.
size
()
==
0
)
{
valid
=
false
;
return
*
this
;
}
else
if
(
segment_candidates
.
size
()
>
1
)
throw
CException
(
_HERE_
,
"Road has multiple paths and no unique subroad exists"
);
else
{
valid
=
true
;
side
=
side_candidates
[
0
];
return
segment_candidates
[
0
];
return
*
segment_candidates
[
0
];
}
}
void
COpendriveRoadSegment
::
load
(
OpenDRIVE
::
road_type
&
road_info
)
{
std
::
map
<
int
,
COpendriveLane
*>::
const_iterator
lane_it
;
...
...
@@ -1034,17 +1103,17 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int
}
}
double
COpendriveRoadSegment
::
get_length
(
void
)
double
COpendriveRoadSegment
::
get_length
(
void
)
const
{
double
length
=
0.0
;
for
(
unsigned
int
i
=
0
;
i
<
this
->
geometries
.
size
();
i
++
)
length
+=
this
->
geometries
[
i
].
opendriv
e
->
get_length
();
length
+=
this
->
geometries
[
i
].
splin
e
->
get_length
();
return
length
;
}
TOpendriveWorldPose
COpendriveRoadSegment
::
get_pose_at
(
double
length
)
TOpendriveWorldPose
COpendriveRoadSegment
::
get_pose_at
(
double
length
)
const
{
TOpendriveWorldPose
world_pose
{
0
};
TPoint
spline_pose
;
...
...
@@ -1066,7 +1135,7 @@ TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length)
return
world_pose
;
}
double
COpendriveRoadSegment
::
get_curvature_at
(
double
length
)
double
COpendriveRoadSegment
::
get_curvature_at
(
double
length
)
const
{
double
curvature
;
TPoint
spline_pose
;
...
...
@@ -1207,12 +1276,18 @@ int COpendriveRoadSegment::get_closest_lane_id(TOpendriveWorldPose &pose,double
return
closest_id
;
}
const
COpendriveLane
&
COpendriveRoadSegment
::
get_closest_lane
(
TOpendriveWorldPose
&
pose
,
double
&
distance
,
double
angle_tol
)
const
COpendriveLane
&
COpendriveRoadSegment
::
get_closest_lane
(
TOpendriveWorldPose
&
pose
,
double
&
distance
,
double
angle_tol
)
const
{
std
::
map
<
int
,
COpendriveLane
*>::
const_iterator
lane_it
;
int
closest_id
=
this
->
get_closest_lane_id
(
pose
,
distance
,
angle_tol
);
if
(
closest_id
==
0
)
throw
CException
(
_HERE_
,
"Impossible to find the closest lane"
);
return
*
this
->
lanes
[
closest_id
];
for
(
lane_it
=
this
->
lanes
.
begin
();
lane_it
!=
this
->
lanes
.
end
();
lane_it
++
)
if
(
lane_it
->
first
==
closest_id
)
return
*
lane_it
->
second
;
throw
CException
(
_HERE_
,
"Impossible to find the closest lane"
);
}
double
COpendriveRoadSegment
::
get_closest_pose
(
TOpendriveWorldPose
&
pose
,
TOpendriveWorldPose
&
closest_pose
,
double
angle_tol
)
const
...
...
@@ -1246,7 +1321,6 @@ double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendr
{
for
(
unsigned
int
i
=
0
;
i
<
closest_index
;
i
++
)
distance
+=
this
->
geometries
[
i
].
spline
->
get_length
();
distance
+=
min_dist
;
}
else
distance
=
std
::
numeric_limits
<
double
>::
max
();
...
...
Write
Preview
Supports
Markdown
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