Skip to content
GitLab
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
59304e44
Commit
59304e44
authored
Jan 01, 2021
by
Sergi Hernandez
Browse files
Solved several bugs to use the correct curvature for the splines.
parent
1a8d4c27
Changes
6
Hide whitespace changes
Inline
Side-by-side
include/opendrive_link.h
View file @
59304e44
...
...
@@ -36,7 +36,6 @@ class COpendriveLink
double
get_resolution
(
void
)
const
;
double
get_scale_factor
(
void
)
const
;
double
find_closest_world_point
(
TOpendriveWorldPoint
&
world
,
TPoint
&
point
);
double
find_closest_local_point
(
TOpendriveLocalPoint
&
local
,
TPoint
&
point
);
double
get_track_point_world
(
TOpendriveTrackPoint
&
track
,
TOpendriveWorldPoint
&
world
);
double
get_track_point_local
(
TOpendriveTrackPoint
&
track
,
TOpendriveLocalPoint
&
local
);
void
get_trajectory
(
std
::
vector
<
double
>
&
x
,
std
::
vector
<
double
>
&
y
,
double
start_length
=
0.0
,
double
end_length
=-
1.0
)
const
;
...
...
include/opendrive_road_segment.h
View file @
59304e44
...
...
@@ -31,6 +31,7 @@ class COpendriveRoadSegment
std
::
vector
<
COpendriveRoadSegment
*>
connecting
;
std
::
string
name
;
unsigned
int
id
;
opendrive_mark_t
center_mark
;
protected:
COpendriveRoadSegment
();
COpendriveRoadSegment
(
const
COpendriveRoadSegment
&
object
,
std
::
map
<
COpendriveRoadNode
*
,
COpendriveRoadNode
*>
&
node_refs
,
COpendriveRoad
*
road_ref
);
...
...
src/opendrive_lane.cpp
View file @
59304e44
...
...
@@ -296,12 +296,10 @@ TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
else
{
track_point
.
t
=
this
->
get_offset
()
+
this
->
get_width
()
/
2.0
;
track_point
.
s
=
0.0
;
for
(
unsigned
int
i
=
0
;
i
<
this
->
nodes
[
this
->
nodes
.
size
()
-
1
]
->
get_num_lanes
();
i
++
)
if
(
&
this
->
nodes
[
this
->
nodes
.
size
()
-
1
]
->
get_lane
(
i
)
==
this
)
{
track_point
.
s
=
this
->
nodes
[
this
->
nodes
.
size
()
-
1
]
->
get_geometry
(
i
).
get_length
();
this
->
nodes
[
this
->
nodes
.
size
()
-
1
]
->
get_geometry
(
i
).
get_world_pose
(
track_point
,
world_point
);
}
}
if
(
this
->
get_id
()
>
0
)
world_point
.
heading
=
normalize_angle
(
world_point
.
heading
+
3.14159
);
...
...
src/opendrive_link.cpp
View file @
59304e44
...
...
@@ -61,7 +61,6 @@ void COpendriveLink::generate(double start_curvature,double end_curvature,double
end
.
y
=
node_end
.
y
;
end
.
heading
=
node_end
.
heading
;
end
.
curvature
=
end_curvature
;
// std::cout << start.x << "," << start.y << "," << start.heading << "," << start_curvature << "," << end.x << "," << end.y << "," << end.heading << "," << end_curvature << std::endl;
this
->
spline
=
new
CG2Spline
(
start
,
end
);
this
->
spline
->
generate
(
this
->
resolution
,
length
);
}
...
...
@@ -99,12 +98,14 @@ double COpendriveLink::get_scale_factor(void) const
double
COpendriveLink
::
find_closest_world_point
(
TOpendriveWorldPoint
&
world
,
TPoint
&
point
)
{
double
length
;
}
double
COpendriveLink
::
find_closest_local_point
(
TOpendriveLocalPoint
&
local
,
TPoint
&
point
)
{
if
(
this
->
spline
!=
NULL
)
length
=
this
->
spline
->
find_closest_point
(
world
.
x
,
world
.
y
,
point
);
else
length
=
std
::
numeric_limits
<
double
>::
max
();
return
length
;
}
double
COpendriveLink
::
get_track_point_world
(
TOpendriveTrackPoint
&
track
,
TOpendriveWorldPoint
&
world
)
...
...
src/opendrive_road_node.cpp
View file @
59304e44
...
...
@@ -61,7 +61,10 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
node_start
=
node
->
get_start_pose
();
for
(
unsigned
int
i
=
0
;
i
<
this
->
lanes
.
size
();
i
++
)
{
lane_end
=
this
->
lanes
[
i
]
->
get_start_point
();
if
(
this
->
lanes
[
i
]
->
get_id
()
<
0
)
lane_end
=
this
->
lanes
[
i
]
->
get_end_point
();
else
lane_end
=
this
->
lanes
[
i
]
->
get_start_point
();
if
(
fabs
(
lane_end
.
x
-
node_start
.
x
)
<
this
->
resolution
&&
fabs
(
lane_end
.
y
-
node_start
.
y
)
<
this
->
resolution
)
{
this
->
geometries
[
i
]
->
get_curvature
(
start_curvature
,
end_curvature
);
...
...
src/opendrive_road_segment.cpp
View file @
59304e44
...
...
@@ -252,7 +252,6 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
void
COpendriveRoadSegment
::
link_neightbour_lanes
(
lanes
::
laneSection_type
&
lane_section
)
{
opendrive_mark_t
center_mark
;
center
::
lane_type
center_lane
;
if
(
lane_section
.
center
().
lane
().
present
())
...
...
@@ -261,30 +260,30 @@ void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_
if
(
center_lane
.
roadMark
().
size
()
>
1
)
throw
CException
(
_HERE_
,
"Only one road mark supported for lane"
);
else
if
(
center_lane
.
roadMark
().
size
()
==
0
)
center_mark
=
OD_MARK_NONE
;
this
->
center_mark
=
OD_MARK_NONE
;
else
if
(
center_lane
.
roadMark
().
size
()
==
1
)
{
if
(
center_lane
.
roadMark
().
begin
()
->
type1
().
present
())
{
if
(
center_lane
.
roadMark
().
begin
()
->
type1
().
get
()
==
"none"
)
center_mark
=
OD_MARK_NONE
;
this
->
center_mark
=
OD_MARK_NONE
;
else
if
(
center_lane
.
roadMark
().
begin
()
->
type1
().
get
()
==
"solid"
)
center_mark
=
OD_MARK_SOLID
;
this
->
center_mark
=
OD_MARK_SOLID
;
else
if
(
center_lane
.
roadMark
().
begin
()
->
type1
().
get
()
==
"broken"
)
center_mark
=
OD_MARK_BROKEN
;
this
->
center_mark
=
OD_MARK_BROKEN
;
else
if
(
center_lane
.
roadMark
().
begin
()
->
type1
().
get
()
==
"solid solid"
)
center_mark
=
OD_MARK_SOLID_SOLID
;
this
->
center_mark
=
OD_MARK_SOLID_SOLID
;
else
if
(
center_lane
.
roadMark
().
begin
()
->
type1
().
get
()
==
"solid broken"
)
center_mark
=
OD_MARK_SOLID_BROKEN
;
this
->
center_mark
=
OD_MARK_SOLID_BROKEN
;
else
if
(
center_lane
.
roadMark
().
begin
()
->
type1
().
get
()
==
"broken solid"
)
center_mark
=
OD_MARK_BROKEN_SOLID
;
this
->
center_mark
=
OD_MARK_BROKEN_SOLID
;
else
if
(
center_lane
.
roadMark
().
begin
()
->
type1
().
get
()
==
"broken broken"
)
center_mark
=
OD_MARK_BROKEN_BROKEN
;
this
->
center_mark
=
OD_MARK_BROKEN_BROKEN
;
else
center_mark
=
OD_MARK_NONE
;
this
->
center_mark
=
OD_MARK_NONE
;
}
else
center_mark
=
OD_MARK_NONE
;
this
->
center_mark
=
OD_MARK_NONE
;
}
}
for
(
int
i
=-
this
->
num_right_lanes
;
i
<
0
;
i
++
)
...
...
@@ -522,7 +521,22 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int
TOpendriveLocalPoint
COpendriveRoadSegment
::
transform_to_local_point
(
const
TOpendriveTrackPoint
&
track
)
{
TOpendriveTrackPoint
point
;
TOpendriveLocalPoint
local
;
if
(
track
.
s
<
0.0
)
throw
CException
(
_HERE_
,
"Invalid track point"
);
point
=
track
;
if
(
this
->
num_right_lanes
>
0
)
local
=
this
->
lanes
[
-
1
]
->
transform_to_local_point
(
point
);
else
{
point
.
s
=
this
->
lanes
[
1
]
->
get_length
()
-
track
.
s
;
point
.
heading
=
normalize_angle
(
track
.
heading
+
3.14159
);
local
=
this
->
lanes
[
1
]
->
transform_to_local_point
(
point
);
}
return
local
;
}
TOpendriveWorldPoint
COpendriveRoadSegment
::
transform_to_world_point
(
const
TOpendriveTrackPoint
&
track
)
...
...
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