Commit 1a8d4c27 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved some bugs to properly generate the splines of each segment and lane.

parent f8dfee4d
......@@ -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);
......
......@@ -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;
......
......@@ -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;
......
......@@ -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);
}
}
}
}
......
......@@ -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);
......
......@@ -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);
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment