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
e8d22de9
Commit
e8d22de9
authored
Jan 09, 2021
by
Sergi Hernandez
Browse files
Solved a bug when the lanes have more than one node.
parent
da181b81
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/opendrive_lane.cpp
View file @
e8d22de9
...
...
@@ -45,7 +45,7 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
if
(
lane
!=
NULL
)
{
if
(
this
->
get_num_nodes
()
!=
lane
->
get_num_nodes
())
throw
CException
(
_HERE_
,
"Impossible to li
i
nk lanes because they have different number of nodes"
);
throw
CException
(
_HERE_
,
"Impossible to link lanes because they have different number of nodes"
);
if
(
this
->
get_num_nodes
()
>
0
&&
lane
->
get_num_nodes
()
>
0
)
{
if
(
this
->
id
*
lane
->
get_id
()
<
0
)
// oposite directions
...
...
@@ -349,7 +349,16 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
for
(
it
=
this
->
nodes
.
begin
(),
i
=
0
;
it
!=
this
->
nodes
.
end
();
i
++
)
{
if
(
i
<
start_node_index
)
{
for
(
exist_it
=
new_node_ref
.
begin
();
exist_it
!=
new_node_ref
.
end
();
exist_it
++
)
if
((
*
it
)
==
exist_it
->
second
)
{
delete
*
it
;
new_node_ref
.
erase
(
exist_it
);
break
;
}
it
=
this
->
nodes
.
erase
(
it
);
}
else
{
exists
=
false
;
...
...
@@ -389,7 +398,16 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &
for
(
it
=
this
->
nodes
.
begin
(),
i
=
0
;
it
!=
this
->
nodes
.
end
();
i
++
)
{
if
(
i
>
end_node_index
)
{
for
(
exist_it
=
new_node_ref
.
begin
();
exist_it
!=
new_node_ref
.
end
();
exist_it
++
)
if
((
*
it
)
==
exist_it
->
second
)
{
delete
*
it
;
new_node_ref
.
erase
(
exist_it
);
break
;
}
it
=
this
->
nodes
.
erase
(
it
);
}
else
{
exists
=
false
;
...
...
src/opendrive_road.cpp
View file @
e8d22de9
...
...
@@ -431,7 +431,6 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
(
*
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
...
...
@@ -771,6 +770,7 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
}
}
new_road
.
update_references
(
new_segment_ref
,
new_lane_ref
,
new_node_ref
);
new_road
.
clean_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++)
...
...
@@ -796,7 +796,6 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
*/
// remove unconnected elements
new_road
.
prune
(
new_path_nodes
);
new_road
.
clean_references
(
new_segment_ref
,
new_lane_ref
,
new_node_ref
);
new_road
.
complete_open_lanes
();
return
new_path_nodes
;
...
...
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