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
cf165668
Commit
cf165668
authored
Jan 09, 2021
by
Sergi Hernandez
Browse files
Improved the way to create the sub road.
parent
e8d22de9
Changes
5
Hide whitespace changes
Inline
Side-by-side
include/opendrive_road.h
View file @
cf165668
...
...
@@ -32,6 +32,7 @@ class COpendriveRoad
void
remove_lane
(
COpendriveLane
*
lane
);
void
complete_open_lanes
(
void
);
void
add_segment
(
COpendriveRoadSegment
*
segment
,
std
::
vector
<
unsigned
int
>
&
old_path
,
std
::
vector
<
unsigned
int
>
&
new_path
);
bool
has_segment
(
COpendriveRoadSegment
*
segment
);
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
);
...
...
include/opendrive_road_segment.h
View file @
cf165668
...
...
@@ -45,6 +45,7 @@ class COpendriveRoadSegment
void
set_id
(
unsigned
int
id
);
void
set_center_mark
(
opendrive_mark_t
mark
);
bool
updated
(
segment_up_ref_t
&
segment_refs
);
COpendriveRoadSegment
*
get_original_segment
(
segment_up_ref_t
&
segment_refs
);
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
add_lanes
(
lanes
::
laneSection_type
&
lane_section
);
...
...
@@ -58,7 +59,7 @@ class COpendriveRoadSegment
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_
node
s
(
COpendriveRoad
Node
*
node1
,
COpendriveRoadNode
*
node
2
);
bool
connects_
segment
s
(
COpendriveRoad
Segment
*
segment1
,
COpendriveRoadSegment
*
segment
2
);
COpendriveRoadSegment
*
get_sub_segment
(
node_up_ref_t
&
new_node_ref
,
lane_up_ref_t
&
new_lane_ref
,
int
node_side
,
TOpendriveWorldPoint
*
start
=
NULL
,
TOpendriveWorldPoint
*
end
=
NULL
);
COpendriveRoadSegment
*
clone
(
node_up_ref_t
&
new_node_ref
,
lane_up_ref_t
&
new_lane_ref
);
public:
...
...
src/opendrive_lane.cpp
View file @
cf165668
...
...
@@ -45,35 +45,20 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
if
(
lane
!=
NULL
)
{
if
(
this
->
get_num_nodes
()
!=
lane
->
get_num_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
)
return
;
if
(
this
->
get_num_nodes
()
==
0
||
lane
->
get_num_nodes
()
==
0
)
return
;
for
(
unsigned
int
i
=
0
;
i
<
this
->
get_num_nodes
()
-
1
;
i
++
)
{
if
(
this
->
id
*
lane
->
get_id
()
<
0
)
// oposite directions
{
for
(
unsigned
int
i
=
0
;
i
<
this
->
get_num_nodes
();
i
++
)
{
this
->
nodes
[
i
]
->
add_link
(
lane
->
nodes
[
lane
->
get_num_nodes
()
-
i
-
1
],
this
->
left_mark
,
this
->
segment
,
this
);
lane
->
nodes
[
lane
->
get_num_nodes
()
-
i
-
1
]
->
add_link
(
this
->
nodes
[
i
],
this
->
left_mark
,
this
->
segment
,
this
);
lane
->
right_mark
=
this
->
left_mark
;
}
}
else
{
for
(
unsigned
int
i
=
0
;
i
<
this
->
get_num_nodes
()
-
1
;
i
++
)
{
this
->
nodes
[
i
]
->
add_link
(
lane
->
nodes
[
i
+
1
],
this
->
left_mark
,
this
->
segment
,
this
);
lane
->
nodes
[
i
]
->
add_link
(
this
->
nodes
[
i
+
1
],
this
->
left_mark
,
this
->
segment
,
this
);
if
(
this
->
id
>
0
)
// left lanes
this
->
right_mark
=
lane
->
left_mark
;
else
// right lanes
this
->
left_mark
=
lane
->
right_mark
;
}
}
this
->
left_lane
=
lane
;
lane
->
right_lane
=
this
;
}
else
throw
CException
(
_HERE_
,
"One of the lanes to link has no nodes"
);
this
->
nodes
[
i
]
->
add_link
(
lane
->
nodes
[
i
+
1
],
this
->
left_mark
,
this
->
segment
,
this
);
lane
->
nodes
[
i
]
->
add_link
(
this
->
nodes
[
i
+
1
],
this
->
left_mark
,
this
->
segment
,
this
);
if
(
this
->
id
>
0
)
// left lanes
this
->
right_mark
=
lane
->
left_mark
;
else
// right lanes
this
->
left_mark
=
lane
->
right_mark
;
}
this
->
left_lane
=
lane
;
lane
->
right_lane
=
this
;
}
}
...
...
src/opendrive_road.cpp
View file @
cf165668
...
...
@@ -266,6 +266,8 @@ void COpendriveRoad::complete_open_lanes(void)
new_node
->
set_index
(
new_node_index
);
(
*
lane_it
)
->
add_node
(
new_node
);
(
*
lane_it
)
->
segment
->
add_node
(
new_node
);
(
*
lane_it
)
->
link_neightbour_lane
((
*
lane_it
)
->
left_lane
);
(
*
lane_it
)
->
link_neightbour_lane
((
*
lane_it
)
->
right_lane
);
}
else
lane_it
++
;
...
...
@@ -275,6 +277,15 @@ void COpendriveRoad::complete_open_lanes(void)
}
}
bool
COpendriveRoad
::
has_segment
(
COpendriveRoadSegment
*
segment
)
{
for
(
unsigned
int
i
=
0
;
i
<
this
->
segments
.
size
();
i
++
)
if
(
this
->
segments
[
i
]
==
segment
)
return
true
;
return
false
;
}
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
++
)
...
...
@@ -455,15 +466,8 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
{
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
;
}
}
if
(
!
this
->
has_segment
(
this
->
nodes
[
i
]
->
links
[
j
]
->
segment
))
this
->
nodes
[
i
]
->
remove_link
(
this
->
nodes
[
i
]
->
links
[
j
]);
}
}
}
...
...
@@ -676,12 +680,11 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
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
,
*
repeat_node
,
*
repeat_next_node
;
COpendriveRoadSegment
*
segment
,
*
new_segment
,
*
repeat_segment
;
COpendriveRoadNode
*
node
,
*
next_node
;
COpendriveRoadSegment
*
segment
,
*
new_segment
,
*
original_seg1
,
original_seg2
;
COpendriveLink
*
link
;
std
::
vector
<
unsigned
int
>
new_path_nodes
;
unsigned
int
link_index
;
bool
added
,
repeated
;
int
node_side
;
new_path_nodes
.
resize
(
path_nodes
.
size
());
...
...
@@ -690,110 +693,60 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
new_road
.
set_scale_factor
(
this
->
scale_factor
);
new_road
.
set_min_road_length
(
this
->
min_road_length
);
if
(
path_nodes
.
size
()
==
1
)
for
(
unsigned
int
i
=
0
;
i
<
path_nodes
.
size
()
-
1
;
i
++
)
{
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
);
node
=
this
->
nodes
[
path_nodes
[
i
]];
next_node
=
this
->
nodes
[
path_nodes
[
i
+
1
]];
link
=
node
->
get_link_with
(
next_node
);
if
(
link
==
NULL
)
throw
CException
(
_HERE_
,
"The provided path has unconnected nodes"
);
segment
=
link
->
segment
;
if
(
new_segment_ref
.
find
(
segment
)
==
new_segment_ref
.
end
())
{
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
);
link
=
node
->
links
[
link_index
];
if
(
link
==
NULL
)
throw
CException
(
_HERE_
,
"The provided path has unconnected nodes"
);
segment
=
link
->
segment
;
if
(
new_segment_ref
.
find
(
segment
)
==
new_segment_ref
.
end
())
{
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
;
}
else
// add additional nodes not explicitly in the path
/*
for(unsigned int i=0;i<this->segments.size();i++)
{
for
(
unsigned
int
i
=
0
;
i
<
path_node
s
.
size
()
-
1
;
i
++
)
for(unsigned int
j
=0;
j<new_road.segment
s.size()
;j
++)
{
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
original_seg1=new_road.segments[j]->get_original_segment(new_segment_ref);
for(unsigned int k=j+1;k<new_road.segments.size();k++)
{
if
(
i
==
0
)
original_seg2=new_road.segments[k]->get_original_segment(new_segment_ref);
if(this->segments[i]->connects_segments(original_seg1,original_seg2)
{
node_side
=
segment
->
get_node_side
(
node
);
repeated
=
false
;
for
(
unsigned
int
j
=
1
;
j
<
path_nodes
.
size
();
j
++
)
if(!new_road.has_segment(new_segment_ref[this->segments[i]]))
{
repeat_node
=
this
->
nodes
[
path_nodes
[
j
]];
if
(
j
==
path_nodes
.
size
()
-
1
)
{
link_index
=
node
->
get_closest_link
(
end_point
,
3.14159
);
repeat_segment
=
node
->
links
[
link_index
]
->
segment
;
}
else
{
repeat_next_node
=
this
->
nodes
[
path_nodes
[
j
+
1
]];
repeat_segment
=
repeat_node
->
get_link_with
(
repeat_next_node
)
->
segment
;
}
if
(
segment
==
repeat_segment
)
{
repeated
=
true
;
break
;
}
new_segment=this->segment[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;
}
if
(
repeated
)
new_segment
=
segment
->
clone
(
new_node_ref
,
new_lane_ref
);
else
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
;
repeated
=
false
;
for
(
unsigned
int
j
=
0
;
j
<
path_nodes
.
size
()
-
1
;
j
++
)
{
repeat_node
=
this
->
nodes
[
path_nodes
[
j
]];
repeat_next_node
=
this
->
nodes
[
path_nodes
[
j
+
1
]];
repeat_segment
=
repeat_node
->
get_link_with
(
repeat_next_node
)
->
segment
;
if
(
segment
==
repeat_segment
)
{
repeated
=
true
;
break
;
}
}
if
(
!
repeated
)
{
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
);
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++)
{
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
.
complete_open_lanes
();
...
...
src/opendrive_road_segment.cpp
View file @
cf165668
...
...
@@ -130,6 +130,17 @@ bool COpendriveRoadSegment::updated(segment_up_ref_t &refs)
return
false
;
}
COpendriveRoadSegment
*
COpendriveRoadSegment
::
get_original_segment
(
segment_up_ref_t
&
segment_refs
)
{
segment_up_ref_t
::
iterator
updated_it
;
for
(
updated_it
=
segment_refs
.
begin
();
updated_it
!=
segment_refs
.
end
();
updated_it
++
)
if
(
updated_it
->
second
==
this
)
return
updated_it
->
first
;
return
NULL
;
}
void
COpendriveRoadSegment
::
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
;
...
...
@@ -481,21 +492,15 @@ void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_
if
(
this
->
lanes
.
find
(
i
+
1
)
!=
this
->
lanes
.
end
())
// if the lane exists
this
->
lanes
[
i
]
->
link_neightbour_lane
(
this
->
lanes
[
i
+
1
]);
else
this
->
lanes
[
i
]
->
left_mark
=
OD_MARK_NONE
;
this
->
lanes
[
i
]
->
left_mark
=
this
->
center_mark
;
}
for
(
int
i
=
this
->
num_left_lanes
;
i
>
0
;
i
--
)
{
if
(
this
->
lanes
.
find
(
i
-
1
)
!=
this
->
lanes
.
end
())
// if the lane exists
this
->
lanes
[
i
]
->
link_neightbour_lane
(
this
->
lanes
[
i
-
1
]);
else
this
->
lanes
[
i
]
->
right_mark
=
OD_MARK_NONE
;
this
->
lanes
[
i
]
->
right_mark
=
this
->
center_mark
;
}
if
(
this
->
lanes
.
find
(
1
)
!=
this
->
lanes
.
end
())
this
->
lanes
[
1
]
->
right_mark
=
this
->
center_mark
;
if
(
this
->
lanes
.
find
(
-
1
)
!=
this
->
lanes
.
end
())
this
->
lanes
[
-
1
]
->
left_mark
=
this
->
center_mark
;
// this->lanes[-1]->link_neightbour_lane(this->lanes[1]);
}
void
COpendriveRoadSegment
::
link_segment
(
COpendriveRoadSegment
&
segment
)
...
...
@@ -574,6 +579,14 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from
if
(
this
->
lanes
.
find
(
from
+
1
)
!=
this
->
lanes
.
end
())
this
->
lanes
[
from
+
1
]
->
link_lane
(
segment
.
lanes
[
to
],
this
->
lanes
[
from
+
1
]
->
left_mark
,
from_start
,
to_start
);
}
/*
if(segment.lanes.find(to-1)!=segment.lanes.end())
if(this->lanes.find(from)!=this->lanes.end())
this->lanes[from]->link_lane(segment.lanes[to-1],this->lanes[from]->right_mark,from_start,to_start);
if(segment.lanes.find(to+1)!=segment.lanes.end())
if(this->lanes.find(from)!=this->lanes.end())
this->lanes[from]->link_lane(segment.lanes[to+1],this->lanes[from]->left_mark,from_start,to_start);
*/
}
bool
COpendriveRoadSegment
::
connects_to
(
COpendriveRoadSegment
*
segment
)
...
...
@@ -585,9 +598,9 @@ bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment)
return
false
;
}
bool
COpendriveRoadSegment
::
connects_
node
s
(
COpendriveRoad
Node
*
node1
,
COpendriveRoadNode
*
node
2
)
bool
COpendriveRoadSegment
::
connects_
segment
s
(
COpendriveRoad
Segment
*
segment1
,
COpendriveRoadSegment
*
segment
2
)
{
if
(
this
->
has_node
(
node1
)
&&
this
->
has_node
(
node
2
))
if
(
this
->
connects_to
(
segment1
)
&&
this
->
connects_to
(
segment
2
))
return
true
;
else
return
false
;
...
...
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