From 1a8d4c2712f1fe465ccffc3058a66c33c6176e85 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 1 Jan 2021 18:32:14 +0100
Subject: [PATCH] Solved some bugs to properly generate the splines of each
segment and lane.
---
include/opendrive_lane.h | 8 ++--
include/opendrive_road_segment.h | 2 +-
src/opendrive_lane.cpp | 66 +++++++++++++++++++++++++++-----
src/opendrive_road.cpp | 19 ++++++++-
src/opendrive_road_node.cpp | 2 +-
src/opendrive_road_segment.cpp | 24 ++++++------
6 files changed, 92 insertions(+), 29 deletions(-)
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index 7873c75..dd94799 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -31,7 +31,7 @@ class COpendriveLane
COpendriveLane(const COpendriveLane &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoadSegment *segment_ref);
void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
void link_neightbour_lane(COpendriveLane *lane);
- void link_lane(COpendriveLane *lane,opendrive_mark_t mark);
+ void link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start,bool to_start);
void add_node(COpendriveRoadNode *node);
void set_resolution(double res);
void set_scale_factor(double scale);
@@ -45,9 +45,9 @@ class COpendriveLane
double get_speed(void) const;
double get_offset(void) const;
int get_id(void) const;
- TOpendriveWorldPoint get_start_point(void);
- TOpendriveWorldPoint get_end_point(void);
- double get_length(void);
+ TOpendriveWorldPoint get_start_point(void) const;
+ TOpendriveWorldPoint get_end_point(void) const;
+ double get_length(void) const;
TOpendriveLocalPoint transform_to_local_point(TOpendriveTrackPoint &track);
TOpendriveWorldPoint transform_to_world_point(TOpendriveTrackPoint &track);
friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index 5834478..90a2431 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -45,7 +45,7 @@ class COpendriveRoadSegment
void add_nodes(OpenDRIVE::road_type &road_info);
void link_neightbour_lanes(lanes::laneSection_type &lane_section);
void link_segment(COpendriveRoadSegment &segment);
- void link_segment(COpendriveRoadSegment &segment,int from, int to);
+ void link_segment(COpendriveRoadSegment &segment,int from,bool from_start, int to,bool to_start);
public:
std::string get_name(void) const;
unsigned int get_id(void) const;
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 5dda2fd..9251711 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -72,12 +72,58 @@ void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
}
}
-void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark)
+void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start, bool to_start)
{
+ COpendriveRoadNode *start,*end;
+
if(lane!=NULL)
{
if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
- this->nodes[this->nodes.size()-1]->add_link(lane->nodes[0],mark);
+ {
+ if(from_start)
+ {
+ if(this->id<0)
+ start=this->nodes[0];
+ else
+ start=this->nodes[this->nodes.size()-1];
+ if(to_start)
+ {
+ if(lane->id<0)
+ end=lane->nodes[0];
+ else
+ end=lane->nodes[lane->nodes.size()-1];
+ }
+ else
+ {
+ if(lane->id<0)
+ end=lane->nodes[lane->nodes.size()-1];
+ else
+ end=lane->nodes[0];
+ }
+ }
+ else
+ {
+ if(this->id<0)
+ start=this->nodes[this->nodes.size()-1];
+ else
+ start=this->nodes[0];
+ if(to_start)
+ {
+ if(lane->id<0)
+ end=lane->nodes[0];
+ else
+ end=lane->nodes[lane->nodes.size()-1];
+ }
+ else
+ {
+ if(lane->id<0)
+ end=lane->nodes[lane->nodes.size()-1];
+ else
+ end=lane->nodes[0];
+ }
+ }
+ start->add_link(end,mark);
+ }
else
throw CException(_HERE_,"One of the lanes to link has no nodes");
}
@@ -233,7 +279,7 @@ int COpendriveLane::get_id(void) const
return this->id;
}
-TOpendriveWorldPoint COpendriveLane::get_start_point(void)
+TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
{
TOpendriveTrackPoint track_point;
TOpendriveWorldPoint world_point;
@@ -263,7 +309,7 @@ TOpendriveWorldPoint COpendriveLane::get_start_point(void)
return world_point;
}
-TOpendriveWorldPoint COpendriveLane::get_end_point(void)
+TOpendriveWorldPoint COpendriveLane::get_end_point(void) const
{
TOpendriveTrackPoint track_point;
TOpendriveWorldPoint world_point;
@@ -272,10 +318,12 @@ TOpendriveWorldPoint COpendriveLane::get_end_point(void)
if(this->id>0)// left lane
{
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)
- this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
+ for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
+ if(&this->nodes[0]->get_lane(i)==this)
+ {
+ track_point.s=this->nodes[0]->get_geometry(i).get_length();;
+ this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
+ }
}
else
{
@@ -293,7 +341,7 @@ TOpendriveWorldPoint COpendriveLane::get_end_point(void)
return world_point;
}
-double COpendriveLane::get_length(void)
+double COpendriveLane::get_length(void) const
{
double length=0.0;
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 0025da6..2f3710d 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -135,8 +135,23 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
to_lane_id=lane_link_it->to().get();
else
throw CException(_HERE_,"Connectivity information missing");
- prev_road.link_segment(road,from_lane_id,-1);
- road.link_segment(next_road,-1,to_lane_id);
+ if(predecessor_contact=="end")
+ prev_road.link_segment(road,from_lane_id,false,-1,true);
+ else
+ prev_road.link_segment(road,from_lane_id,true,-1,true);
+ TOpendriveWorldPoint end=road.get_lane(-1).get_end_point();
+ if(successor_contact=="end")
+ {
+ TOpendriveWorldPoint start=next_road.get_lane(to_lane_id).get_end_point();
+ if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
+ road.link_segment(next_road,-1,false,to_lane_id,false);
+ }
+ else
+ {
+ TOpendriveWorldPoint start=next_road.get_lane(to_lane_id).get_start_point();
+ if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
+ road.link_segment(next_road,-1,false,to_lane_id,true);
+ }
}
}
}
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index eb91077..5003ef5 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -61,7 +61,7 @@ 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_end_point();
+ 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);
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 4a4bd78..fccf826 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -207,8 +207,8 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
new_node->set_parent_segment(this);
}
this->lanes[i]->add_node(new_node);
- lane_offset-=this->lanes[i]->width;
}
+ lane_offset-=this->lanes[i]->width;
}
}
for(geom_it=road_info.planView().geometry().end(); geom_it!=road_info.planView().geometry().begin();)
@@ -238,8 +238,8 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
new_node->set_parent_segment(this);
}
this->lanes[i]->add_node(new_node);
- lane_offset+=this->lanes[i]->width;
}
+ lane_offset+=this->lanes[i]->width;
}
}
}catch(CException &e){
@@ -329,11 +329,11 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment)
if(segment.lanes.find(j)!=segment.lanes.end())
{
if(j==i-1)
- this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->right_mark);
+ this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->right_mark,false,true);
else if(j==i)
- this->lanes[i]->link_lane(segment.lanes[j],OD_MARK_NONE);
+ this->lanes[i]->link_lane(segment.lanes[j],OD_MARK_NONE,false,true);
else
- this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->left_mark);
+ this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->left_mark,false,true);
}
}
}
@@ -344,17 +344,17 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment)
if(this->lanes.find(j)!=this->lanes.end())
{
if(j==i-1)
- segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->right_mark);
+ segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->right_mark,false,true);
else if(j==i)
- segment.lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE);
+ segment.lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true);
else
- segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->left_mark);
+ segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->left_mark,false,true);
}
}
}
}
-void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from, int to)
+void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from,bool from_start,int to,bool to_start)
{
bool connect=true;
@@ -381,11 +381,11 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from
if(segment.lanes.find(to)!=segment.lanes.end())
{
if(this->lanes.find(from-1)!=this->lanes.end())
- this->lanes[from-1]->link_lane(segment.lanes[to],this->lanes[from-1]->right_mark);
+ this->lanes[from-1]->link_lane(segment.lanes[to],this->lanes[from-1]->right_mark,from_start,to_start);
if(this->lanes.find(from)!=this->lanes.end())
- this->lanes[from]->link_lane(segment.lanes[to],OD_MARK_NONE);
+ this->lanes[from]->link_lane(segment.lanes[to],OD_MARK_NONE,from_start,to_start);
if(this->lanes.find(from+1)!=this->lanes.end())
- this->lanes[from+1]->link_lane(segment.lanes[to],this->lanes[from+1]->left_mark);
+ this->lanes[from+1]->link_lane(segment.lanes[to],this->lanes[from+1]->left_mark,from_start,to_start);
}
}
--
GitLab