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
db6473e6
Commit
db6473e6
authored
Jan 08, 2021
by
Sergi Hernandez
Browse files
Added functions to get a part of the road.
Added functions to update all the references when a new road is created.
parent
863db51e
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/opendrive_road.h
View file @
db6473e6
...
...
@@ -5,28 +5,37 @@
#include
"xml/OpenDRIVE_1.4H.hxx"
#endif
#include
"opendrive_common.h"
#include
"opendrive_road_segment.h"
#include
"opendrive_road_node.h"
class
COpendriveRoadSegment
;
class
COpendriveRoadNode
;
#include
"opendrive_lane.h"
class
COpendriveRoad
{
friend
class
COpendriveRoadSegment
;
friend
class
COpendriveRoadNode
;
friend
class
COpendriveLane
;
private:
std
::
vector
<
COpendriveRoadSegment
*>
segments
;
std
::
vector
<
COpendriveRoadNode
*>
nodes
;
std
::
vector
<
COpendriveLane
*>
lanes
;
double
resolution
;
double
scale_factor
;
double
min_road_length
;
protected:
COpendriveRoadSegment
&
operator
[](
std
::
string
&
key
);
void
free
(
void
);
void
link_segments
(
OpenDRIVE
&
open_drive
);
unsigned
int
add_node
(
COpendriveRoadNode
*
node
);
bool
node_exists_at
(
const
TOpendriveWorldPoint
&
pose
);
COpendriveRoadNode
*
get_node_at
(
const
TOpendriveWorldPoint
&
pose
)
const
;
void
remove_node
(
COpendriveRoadNode
*
node
);
unsigned
int
add_lane
(
COpendriveLane
*
lane
);
void
remove_lane
(
COpendriveLane
*
lane
);
void
add_segment
(
COpendriveRoadSegment
*
segment
,
std
::
vector
<
unsigned
int
>
&
old_path
,
std
::
vector
<
unsigned
int
>
&
new_path
);
void
update_references
(
segment_up_ref_t
&
segment_refs
,
lane_up_ref_t
&
lane_refs
,
node_up_ref_t
&
node_refs
);
void
clean_references
(
segment_up_ref_t
&
segment_refs
,
lane_up_ref_t
&
lane_refs
,
node_up_ref_t
&
node_refs
);
void
prune
(
std
::
vector
<
unsigned
int
>
&
path_nodes
);
bool
node_exists_at
(
TOpendriveWorldPoint
&
pose
);
COpendriveRoadNode
*
get_node_at
(
TOpendriveWorldPoint
&
pose
);
public:
COpendriveRoad
();
COpendriveRoad
(
const
COpendriveRoad
&
object
);
...
...
@@ -42,9 +51,11 @@ class COpendriveRoad
unsigned
int
get_num_nodes
(
void
)
const
;
const
COpendriveRoadNode
&
get_node
(
unsigned
int
index
)
const
;
const
COpendriveRoadSegment
&
operator
[](
std
::
size_t
index
);
unsigned
int
get_closest_node
(
double
x
,
double
y
,
double
heading
,
double
angle_tol
=
0.1
);
double
get_closest_point
(
double
x
,
double
y
,
double
heading
,
TOpendriveWorldPoint
&
closest_point
,
double
angle_tol
=
0.1
);
void
get_closest_points
(
double
x
,
double
y
,
double
heading
,
std
::
vector
<
TOpendriveWorldPoint
>
&
closest_points
,
std
::
vector
<
double
>
&
closest_lengths
,
double
dist_tol
,
double
angle_tol
=
0.1
);
unsigned
int
get_closest_node
(
TOpendriveWorldPoint
&
point
,
double
angle_tol
=
0.1
);
double
get_closest_point
(
TOpendriveWorldPoint
&
point
,
TOpendriveWorldPoint
&
closest_point
,
double
angle_tol
=
0.1
);
void
get_closest_points
(
TOpendriveWorldPoint
&
point
,
std
::
vector
<
TOpendriveWorldPoint
>
&
closest_points
,
std
::
vector
<
double
>
&
closest_lengths
,
double
dist_tol
,
double
angle_tol
=
0.1
);
void
get_sub_road
(
std
::
vector
<
unsigned
int
>
&
path_nodes
,
TOpendriveWorldPoint
&
start_point
,
TOpendriveWorldPoint
&
end_point
,
COpendriveRoad
&
new_road
);
void
get_sub_road
(
TOpendriveWorldPoint
&
start_point
,
double
length
,
COpendriveRoad
&
new_road
);
void
operator
=
(
const
COpendriveRoad
&
object
);
friend
std
::
ostream
&
operator
<<
(
std
::
ostream
&
out
,
COpendriveRoad
&
road
);
~
COpendriveRoad
();
...
...
src/opendrive_road.cpp
View file @
db6473e6
...
...
@@ -11,41 +11,41 @@ COpendriveRoad::COpendriveRoad()
this
->
min_road_length
=
DEFAULT_MIN_ROAD_LENGTH
;
this
->
segments
.
clear
();
this
->
nodes
.
clear
();
this
->
lanes
.
clear
();
}
COpendriveRoad
::
COpendriveRoad
(
const
COpendriveRoad
&
object
)
{
COpendriveRoadSegment
*
segment
;
COpendriveRoadNode
*
node
;
std
::
map
<
COpendriveRoadSegment
*
,
COpendriveRoadSegment
*>
new_segment_ref
;
std
::
map
<
COpendriveRoadNode
*
,
COpendriveRoadNode
*>
new_node_ref
;
COpendriveLane
*
lane
;
segment_up_ref_t
new_segment_ref
;
node_up_ref_t
new_node_ref
;
lane_up_ref_t
new_lane_ref
;
this
->
free
();
this
->
resolution
=
object
.
resolution
;
this
->
scale_factor
=
object
.
scale_factor
;
this
->
min_road_length
=
object
.
min_road_length
;
this
->
segments
.
clear
();
for
(
unsigned
int
i
=
0
;
i
<
object
.
lanes
.
size
();
i
++
)
{
lane
=
new
COpendriveLane
(
*
object
.
lanes
[
i
]);
this
->
lanes
.
push_back
(
lane
);
new_lane_ref
[
object
.
lanes
[
i
]]
=
lane
;
}
for
(
unsigned
int
i
=
0
;
i
<
object
.
nodes
.
size
();
i
++
)
{
node
=
new
COpendriveRoadNode
(
*
object
.
nodes
[
i
]);
this
->
nodes
.
push_back
(
node
);
new_node_ref
[
object
.
nodes
[
i
]]
=
node
;
}
// update references
for
(
unsigned
int
i
=
0
;
i
<
this
->
nodes
.
size
();
i
++
)
this
->
nodes
[
i
]
->
update_references
(
new_node_ref
);
for
(
unsigned
int
i
=
0
;
i
<
object
.
segments
.
size
();
i
++
)
{
segment
=
new
COpendriveRoadSegment
(
*
object
.
segments
[
i
]
,
new_node_ref
,
this
);
segment
=
new
COpendriveRoadSegment
(
*
object
.
segments
[
i
]);
this
->
segments
.
push_back
(
segment
);
new_segment_ref
[
object
.
segments
[
i
]]
=
segment
;
}
this
->
nodes
.
clear
();
// update references
for
(
unsigned
int
i
=
0
;
i
<
this
->
segments
.
size
();
i
++
)
this
->
segments
[
i
]
->
update_references
(
new_segment_ref
);
for
(
unsigned
int
i
=
0
;
i
<
this
->
nodes
.
size
();
i
++
)
this
->
nodes
[
i
]
->
update_references
(
new_segment_ref
);
}
COpendriveRoadSegment
&
COpendriveRoad
::
operator
[](
std
::
string
&
key
)
...
...
@@ -59,6 +59,28 @@ COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
throw
CException
(
_HERE_
,
"No road segment with the given ID"
);
}
void
COpendriveRoad
::
free
(
void
)
{
for
(
unsigned
int
i
=
0
;
i
<
this
->
segments
.
size
();
i
++
)
{
delete
this
->
segments
[
i
];
this
->
segments
[
i
]
=
NULL
;
}
this
->
segments
.
clear
();
for
(
unsigned
int
i
=
0
;
i
<
this
->
nodes
.
size
();
i
++
)
{
delete
this
->
nodes
[
i
];
this
->
nodes
[
i
]
=
NULL
;
}
this
->
nodes
.
clear
();
for
(
unsigned
int
i
=
0
;
i
<
this
->
lanes
.
size
();
i
++
)
{
delete
this
->
lanes
[
i
];
this
->
lanes
[
i
]
=
NULL
;
}
this
->
lanes
.
clear
();
}
void
COpendriveRoad
::
link_segments
(
OpenDRIVE
&
open_drive
)
{
std
::
string
predecessor_id
,
successor_id
;
...
...
@@ -172,7 +194,237 @@ unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node)
return
this
->
nodes
.
size
()
-
1
;
}
bool
COpendriveRoad
::
node_exists_at
(
const
TOpendriveWorldPoint
&
pose
)
void
COpendriveRoad
::
remove_node
(
COpendriveRoadNode
*
node
)
{
std
::
vector
<
COpendriveRoadNode
*>::
iterator
it
;
for
(
it
=
this
->
nodes
.
begin
();
it
!=
this
->
nodes
.
end
();)
{
if
((
*
it
)
->
get_index
()
==
node
->
get_index
())
{
delete
*
it
;
it
=
this
->
nodes
.
erase
(
it
);
break
;
}
else
it
++
;
}
}
unsigned
int
COpendriveRoad
::
add_lane
(
COpendriveLane
*
lane
)
{
for
(
unsigned
int
i
=
0
;
i
<
this
->
lanes
.
size
();
i
++
)
{
if
(
this
->
lanes
[
i
]
==
lane
)
throw
CException
(
_HERE_
,
"Lane already present"
);
}
this
->
lanes
.
push_back
(
lane
);
return
this
->
lanes
.
size
()
-
1
;
}
void
COpendriveRoad
::
remove_lane
(
COpendriveLane
*
lane
)
{
std
::
vector
<
COpendriveLane
*>::
iterator
it
;
for
(
it
=
this
->
lanes
.
begin
();
it
!=
this
->
lanes
.
end
();)
{
if
((
*
it
)
->
get_index
()
==
lane
->
get_index
())
{
delete
*
it
;
it
=
this
->
lanes
.
erase
(
it
);
break
;
}
else
it
++
;
}
}
void
COpendriveRoad
::
add_segment
(
COpendriveRoadSegment
*
segment
,
std
::
vector
<
unsigned
int
>
&
old_path
,
std
::
vector
<
unsigned
int
>
&
new_path
)
{
for
(
unsigned
int
i
=
0
;
i
<
this
->
segments
.
size
();
i
++
)
if
(
this
->
segments
[
i
]
->
get_id
()
==
segment
->
get_id
())
// segment is already present
return
;
// add the new segment
this
->
segments
.
push_back
(
segment
);
// add the lanes
for
(
unsigned
int
i
=
1
;
i
<=
segment
->
get_num_right_lanes
();
i
++
)
{
segment
->
lanes
[
-
i
]
->
set_index
(
this
->
lanes
.
size
());
this
->
lanes
.
push_back
(
segment
->
lanes
[
-
i
]);
}
for
(
unsigned
int
i
=
1
;
i
<=
segment
->
get_num_left_lanes
();
i
++
)
{
segment
->
lanes
[
i
]
->
set_index
(
this
->
lanes
.
size
());
this
->
lanes
.
push_back
(
segment
->
lanes
[
i
]);
}
// add the nodes
for
(
unsigned
int
i
=
0
;
i
<
segment
->
get_num_nodes
();
i
++
)
{
for
(
unsigned
int
j
=
0
;
j
<
old_path
.
size
();
j
++
)
if
(
old_path
[
j
]
==
segment
->
nodes
[
i
]
->
index
)
new_path
.
push_back
(
this
->
nodes
.
size
());
segment
->
nodes
[
i
]
->
set_index
(
this
->
nodes
.
size
());
this
->
nodes
.
push_back
(
segment
->
nodes
[
i
]);
}
}
void
COpendriveRoad
::
update_references
(
segment_up_ref_t
&
segment_refs
,
lane_up_ref_t
&
lane_refs
,
node_up_ref_t
&
node_refs
)
{
std
::
vector
<
COpendriveRoadSegment
*>::
iterator
seg_it
;
std
::
vector
<
COpendriveLane
*>::
iterator
lane_it
;
std
::
vector
<
COpendriveRoadNode
*>::
iterator
node_it
;
for
(
seg_it
=
this
->
segments
.
begin
();
seg_it
!=
this
->
segments
.
end
();
seg_it
++
)
{
if
(
segment_refs
.
find
(
*
seg_it
)
!=
segment_refs
.
end
())
{
(
*
seg_it
)
=
segment_refs
[
*
seg_it
];
(
*
seg_it
)
->
update_references
(
segment_refs
,
lane_refs
,
node_refs
);
}
else
if
((
*
seg_it
)
->
updated
(
segment_refs
))
(
*
seg_it
)
->
update_references
(
segment_refs
,
lane_refs
,
node_refs
);
}
for
(
lane_it
=
this
->
lanes
.
begin
();
lane_it
!=
this
->
lanes
.
end
();
lane_it
++
)
if
(
lane_refs
.
find
(
*
lane_it
)
!=
lane_refs
.
end
())
(
*
lane_it
)
=
lane_refs
[
*
lane_it
];
for
(
node_it
=
this
->
nodes
.
begin
();
node_it
!=
this
->
nodes
.
end
();
node_it
++
)
if
(
node_refs
.
find
(
*
node_it
)
!=
node_refs
.
end
())
(
*
node_it
)
=
node_refs
[
*
node_it
];
}
void
COpendriveRoad
::
clean_references
(
segment_up_ref_t
&
segment_refs
,
lane_up_ref_t
&
lane_refs
,
node_up_ref_t
&
node_refs
)
{
std
::
vector
<
COpendriveRoadSegment
*>::
iterator
seg_it
;
std
::
vector
<
COpendriveLane
*>::
iterator
lane_it
;
std
::
vector
<
COpendriveRoadNode
*>::
iterator
node_it
;
for
(
seg_it
=
this
->
segments
.
begin
();
seg_it
!=
this
->
segments
.
end
();)
{
if
(
segment_refs
.
find
(
*
seg_it
)
!=
segment_refs
.
end
())
{
(
*
seg_it
)
=
segment_refs
[
*
seg_it
];
(
*
seg_it
)
->
clean_references
(
segment_refs
,
lane_refs
,
node_refs
);
seg_it
++
;
}
else
if
((
*
seg_it
)
->
updated
(
segment_refs
))
{
(
*
seg_it
)
->
clean_references
(
segment_refs
,
lane_refs
,
node_refs
);
seg_it
++
;
}
else
seg_it
=
this
->
segments
.
erase
(
seg_it
);
}
for
(
lane_it
=
this
->
lanes
.
begin
();
lane_it
!=
this
->
lanes
.
end
();)
{
if
(
lane_refs
.
find
(
*
lane_it
)
!=
lane_refs
.
end
())
{
(
*
lane_it
)
=
lane_refs
[
*
lane_it
];
lane_it
++
;
}
else
if
(
!
(
*
lane_it
)
->
updated
(
lane_refs
))
lane_it
=
this
->
lanes
.
erase
(
lane_it
);
else
lane_it
++
;
}
for
(
node_it
=
this
->
nodes
.
begin
();
node_it
!=
this
->
nodes
.
end
();)
{
if
(
node_refs
.
find
(
*
node_it
)
!=
node_refs
.
end
())
{
(
*
node_it
)
=
node_refs
[
*
node_it
];
node_it
++
;
}
else
if
(
!
(
*
node_it
)
->
updated
(
node_refs
))
node_it
=
this
->
nodes
.
erase
(
node_it
);
else
node_it
++
;
}
}
void
COpendriveRoad
::
prune
(
std
::
vector
<
unsigned
int
>
&
path_nodes
)
{
COpendriveLane
*
neightbour_lane
;
std
::
vector
<
COpendriveLane
*>::
iterator
lane_it
;
bool
present
;
// remove all nodes and lanes not present in the path
for
(
lane_it
=
this
->
lanes
.
begin
();
lane_it
!=
this
->
lanes
.
end
();)
{
present
=
false
;
for
(
unsigned
int
i
=
0
;
i
<
path_nodes
.
size
();
i
++
)
{
if
((
*
lane_it
)
->
has_node
(
path_nodes
[
i
]))
{
present
=
true
;
break
;
}
}
if
(
!
present
)
{
neightbour_lane
=
(
*
lane_it
)
->
left_lane
;
while
(
neightbour_lane
!=
NULL
)
{
for
(
unsigned
int
i
=
0
;
i
<
path_nodes
.
size
();
i
++
)
{
if
(
neightbour_lane
->
has_node
(
path_nodes
[
i
]))
{
present
=
true
;
break
;
}
}
if
(
present
)
break
;
neightbour_lane
=
neightbour_lane
->
left_lane
;
}
neightbour_lane
=
(
*
lane_it
)
->
right_lane
;
while
(
neightbour_lane
!=
NULL
)
{
for
(
unsigned
int
i
=
0
;
i
<
path_nodes
.
size
();
i
++
)
{
if
(
neightbour_lane
->
has_node
(
path_nodes
[
i
]))
{
present
=
true
;
break
;
}
}
if
(
present
)
break
;
neightbour_lane
=
neightbour_lane
->
right_lane
;
}
if
(
!
present
)
{
(
*
lane_it
)
->
segment
->
remove_lane
(
*
lane_it
);
for
(
unsigned
int
i
=
0
;
i
<
(
*
lane_it
)
->
nodes
.
size
();
i
++
)
this
->
remove_node
((
*
lane_it
)
->
nodes
[
i
]);
delete
*
lane_it
;
lane_it
=
this
->
lanes
.
erase
(
lane_it
);
}
else
lane_it
++
;
}
else
lane_it
++
;
}
// remove links to non-consecutive nodes
for
(
unsigned
int
i
=
0
;
i
<
this
->
nodes
.
size
();
i
++
)
{
for
(
unsigned
int
j
=
0
;
j
<
this
->
nodes
[
i
]
->
get_num_links
();
j
++
)
{
for
(
unsigned
int
k
=
0
;
k
<
path_nodes
.
size
()
-
1
;
k
++
)
{
if
(
path_nodes
[
k
]
==
this
->
nodes
[
i
]
->
links
[
j
]
->
prev
->
index
)
if
(
path_nodes
[
k
+
1
]
!=
this
->
nodes
[
i
]
->
links
[
j
]
->
next
->
index
)
{
this
->
nodes
[
i
]
->
remove_link
(
this
->
nodes
[
i
]
->
links
[
j
]);
break
;
}
}
}
}
}
bool
COpendriveRoad
::
node_exists_at
(
TOpendriveWorldPoint
&
pose
)
{
TOpendriveWorldPoint
node_pose
;
...
...
@@ -186,7 +438,7 @@ bool COpendriveRoad::node_exists_at(const TOpendriveWorldPoint &pose)
return
false
;
}
COpendriveRoadNode
*
COpendriveRoad
::
get_node_at
(
const
TOpendriveWorldPoint
&
pose
)
const
COpendriveRoadNode
*
COpendriveRoad
::
get_node_at
(
TOpendriveWorldPoint
&
pose
)
{
TOpendriveWorldPoint
node_pose
;
...
...
@@ -200,6 +452,7 @@ COpendriveRoadNode* COpendriveRoad::get_node_at(const TOpendriveWorldPoint &pose
return
NULL
;
}
void
COpendriveRoad
::
load
(
const
std
::
string
&
filename
)
{
struct
stat
buffer
;
...
...
@@ -207,9 +460,7 @@ void COpendriveRoad::load(const std::string &filename)
if
(
stat
(
filename
.
c_str
(),
&
buffer
)
==
0
)
{
// delete any previous data
for
(
unsigned
int
i
=
0
;
i
<
this
->
segments
.
size
();
i
++
)
delete
this
->
segments
[
i
];
this
->
segments
.
clear
();
this
->
free
();
// try to open the specified file
try
{
std
::
unique_ptr
<
OpenDRIVE
>
open_drive
(
OpenDRIVE_
(
filename
.
c_str
(),
xml_schema
::
flags
::
dont_validate
));
...
...
@@ -317,16 +568,16 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
}
unsigned
int
COpendriveRoad
::
get_closest_node
(
double
x
,
double
y
,
double
head
in
g
,
double
angle_tol
)
unsigned
int
COpendriveRoad
::
get_closest_node
(
TOpendriveWorldPoint
&
po
in
t
,
double
angle_tol
)
{
double
dist
,
min_dist
=
std
::
numeric_limits
<
double
>::
max
();
TOpendriveWorldPoint
point
;
TOpendriveWorldPoint
closest_
point
;
unsigned
int
closest_index
;
for
(
unsigned
int
i
=
0
;
i
<
this
->
nodes
.
size
();
i
++
)
{
this
->
nodes
[
i
]
->
get_closest_point
(
x
,
y
,
heading
,
point
,
angle_tol
);
dist
=
sqrt
(
pow
(
point
.
x
-
x
,
2.0
)
+
pow
(
point
.
y
-
y
,
2.0
));
this
->
nodes
[
i
]
->
get_closest_point
(
point
,
closest_
point
,
angle_tol
);
dist
=
sqrt
(
pow
(
closest_
point
.
x
-
point
.
x
,
2.0
)
+
pow
(
closest_
point
.
y
-
point
.
y
,
2.0
));
if
(
dist
<
min_dist
)
{
min_dist
=
dist
;
...
...
@@ -337,20 +588,20 @@ unsigned int COpendriveRoad::get_closest_node(double x, double y,double heading,
return
closest_index
;
}
double
COpendriveRoad
::
get_closest_point
(
double
x
,
double
y
,
double
head
in
g
,
TOpendriveWorldPoint
&
closest_point
,
double
angle_tol
)
double
COpendriveRoad
::
get_closest_point
(
TOpendriveWorldPoint
&
po
in
t
,
TOpendriveWorldPoint
&
closest_point
,
double
angle_tol
)
{
double
dist
,
min_dist
=
std
::
numeric_limits
<
double
>::
max
();
TOpendriveWorldPoint
point
;
TOpendriveWorldPoint
point
_found
;
double
length
,
closest_length
;
for
(
unsigned
int
i
=
0
;
i
<
this
->
nodes
.
size
();
i
++
)
{
length
=
this
->
nodes
[
i
]
->
get_closest_point
(
x
,
y
,
head
in
g
,
point
,
angle_tol
);
dist
=
sqrt
(
pow
(
point
.
x
-
x
,
2.0
)
+
pow
(
point
.
y
-
y
,
2.0
));
length
=
this
->
nodes
[
i
]
->
get_closest_point
(
po
in
t
,
point
_found
,
angle_tol
);
dist
=
sqrt
(
pow
(
point
_found
.
x
-
point
.
x
,
2.0
)
+
pow
(
point
_found
.
y
-
point
.
y
,
2.0
));
if
(
dist
<
min_dist
)
{
min_dist
=
dist
;
closest_point
=
point
;
closest_point
=
point
_found
;
closest_length
=
length
;
}
}
...
...
@@ -358,7 +609,7 @@ double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpen
return
closest_length
;
}
void
COpendriveRoad
::
get_closest_points
(
double
x
,
double
y
,
double
head
in
g
,
std
::
vector
<
TOpendriveWorldPoint
>
&
closest_points
,
std
::
vector
<
double
>
&
closest_lengths
,
double
dist_tol
,
double
angle_tol
)
void
COpendriveRoad
::
get_closest_points
(
TOpendriveWorldPoint
&
po
in
t
,
std
::
vector
<
TOpendriveWorldPoint
>
&
closest_points
,
std
::
vector
<
double
>
&
closest_lengths
,
double
dist_tol
,
double
angle_tol
)
{
std
::
vector
<
TOpendriveWorldPoint
>
points
;
std
::
vector
<
double
>
lengths
;
...
...
@@ -367,7 +618,7 @@ void COpendriveRoad::get_closest_points(double x, double y,double heading,std::v
closest_lengths
.
clear
();
for
(
unsigned
int
i
=
0
;
i
<
this
->
nodes
.
size
();
i
++
)
{
this
->
nodes
[
i
]
->
get_closest_points
(
x
,
y
,
head
in
g
,
points
,
lengths
,
dist_tol
,
angle_tol
);
this
->
nodes
[
i
]
->
get_closest_points
(
po
in
t
,
points
,
lengths
,
dist_tol
,
angle_tol
);
for
(
unsigned
int
j
=
0
;
j
<
points
.
size
();
i
++
)
{
closest_points
.
push_back
(
points
[
i
]);
...
...
@@ -376,40 +627,132 @@ void COpendriveRoad::get_closest_points(double x, double y,double heading,std::v
}
}
void
COpendriveRoad
::
get_sub_road
(
std
::
vector
<
unsigned
int
>
&
path_nodes
,
TOpendriveWorldPoint
&
start_point
,
TOpendriveWorldPoint
&
end_point
,
COpendriveRoad
&
new_road
)
{
segment_up_ref_t
new_segment_ref
;
lane_up_ref_t
new_lane_ref
;
node_up_ref_t
new_node_ref
;
COpendriveRoadNode
*
node
,
*
next_node
;
COpendriveRoadSegment
*
segment
,
*
new_segment
;
std
::
vector
<
unsigned
int
>
new_path_nodes
;
unsigned
int
link_index
;
int
node_side
;
bool
added
;
new_road
.
free
();
new_road
.
set_resolution
(
this
->
resolution
);
new_road
.
set_scale_factor
(
this
->
scale_factor
);
new_road
.
set_min_road_length
(
this
->
min_road_length
);
if
(
path_nodes
.
size
()
==
1
)
{
node
=
this
->
nodes
[
path_nodes
[
path_nodes
.
size
()
-
1
]];
link_index
=
node
->
get_closest_link
(
end_point
);
segment
=
node
->
links
[
link_index
]
->
segment
;
node_side
=
segment
->
get_node_side
(
node
);
new_segment
=
segment
->
get_sub_segment
(
new_node_ref
,
new_lane_ref
,
node_side
,
&
start_point
,
&
end_point
);
new_road
.
add_segment
(
new_segment
,
path_nodes
,
new_path_nodes
);
new_segment_ref
[
segment
]
=
new_segment
;
}
else
{
for
(
unsigned
int
i
=
0
;
i
<
path_nodes
.
size
()
-
1
;
i
++
)
{
node
=
this
->
nodes
[
path_nodes
[
i
]];
next_node
=
this
->
nodes
[
path_nodes
[
i
+
1
]];
segment
=
node
->
get_link_with
(
next_node
)
->
segment
;
if
(
segment
->
has_node
(
next_node
))
continue
;
else
{
if
(
i
==
0
)
{
node_side
=
segment
->
get_node_side
(
node
);
new_segment
=
segment
->
get_sub_segment
(
new_node_ref
,
new_lane_ref
,
node_side
,
&
start_point
,
NULL
);
}
else
new_segment
=
segment
->
clone
(
new_node_ref
,
new_lane_ref
);
new_road
.
add_segment
(
new_segment
,
path_nodes
,
new_path_nodes
);
new_segment_ref
[
segment
]
=
new_segment
;
}
}
// add the last segment
node
=
this
->
nodes
[
path_nodes
[
path_nodes
.
size
()
-
1
]];
link_index
=
node
->
get_closest_link
(
end_point
,
3.14159
);
segment
=
node
->
links
[
link_index
]
->
segment
;
node_side
=
segment
->
get_node_side
(
node
);
new_segment
=
segment
->
get_sub_segment
(
new_node_ref
,
new_lane_ref
,
node_side
,
NULL
,
&
end_point
);
new_road
.
add_segment
(
new_segment
,
path_nodes
,
new_path_nodes
);
new_segment_ref
[
segment
]
=
new_segment
;
}
new_road
.
update_references
(
new_segment_ref
,
new_lane_ref
,
new_node_ref
);
// add additional nodes not explicitly in the path
/*
for(unsigned int i=0;i<path_nodes.size();i++)
{
added=false;
node=this->nodes[path_nodes[i]];
for(unsigned int j=i+1;j<path_nodes.size();j++)
{
next_node=this->nodes[path_nodes[j]];
for(unsigned int k=0;k<this->segments.size();k++)
if(this->segments[k]->connects_nodes(node,next_node))
{
new_segment=this->segments[k]->clone(new_node_ref,new_lane_ref);
new_road.add_segment(new_segment,path_nodes,new_path_nodes);
new_segment_ref[segment]=new_segment;
added=true;
break;
}
if(added)
break;
}
}
*/
// remove unconnected elements
new_road
.
prune
(
new_path_nodes
);
new_road
.
clean_references
(
new_segment_ref
,
new_lane_ref
,
new_node_ref
);
}
void
COpendriveRoad
::
get_sub_road
(
TOpendriveWorldPoint
&
start_point
,
double
length
,
COpendriveRoad
&
road
)
{
}
void
COpendriveRoad
::
operator
=
(
const
COpendriveRoad
&
object
)
{
COpendriveRoadSegment
*
segment
;
COpendriveRoadNode
*
node
;
std
::
map
<
COpendriveRoadSegment
*
,
COpendriveRoadSegment
*>
new_references
;
COpendriveLane
*
lane
;
segment_up_ref_t
new_segment_ref
;
node_up_ref_t
new_node_ref
;
lane_up_ref_t
new_lane_ref
;
this
->
free
();
this
->
resolution
=
object
.
resolution
;
this
->
scale_factor
=
object
.
scale_factor
;
this
->
min_road_length
=
object
.
min_road_length
;
for
(
unsigned
int
i
=
0
;
i
<
this
->
segment
s
.
size
();
i
++
)
for
(
unsigned
int
i
=
0
;
i
<
object
.
lane
s
.
size
();
i
++
)
{
delete
this
->
segments
[
i
];
this
->
segments
[
i
]
=
NULL
;
lane
=
new
COpendriveLane
(
*
object
.
lanes
[
i
]);
this
->
lanes
.
push_back
(
lane
);
new_lane_ref
[
object
.
lanes
[
i
]]
=
lane
;
}
this
->
segments
.
clear
();
for
(
unsigned
int
i
=
0
;
i
<
object
.
segments
.
size
();
i
++
)
{
segment
=
new
COpendriveRoadSegment
(
*
object
.
segments
[
i
]);
this
->
segments
.
push_back
(
segment
);
new_references
[
object
.
segments
[
i
]]
=
segment
;
}