From 59304e44c6924e8d9f985e9b83679c4d879f72e6 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 1 Jan 2021 19:27:52 +0100
Subject: [PATCH] Solved several bugs to use the correct curvature for the
splines.
---
include/opendrive_link.h | 1 -
include/opendrive_road_segment.h | 1 +
src/opendrive_lane.cpp | 4 +---
src/opendrive_link.cpp | 11 +++++-----
src/opendrive_road_node.cpp | 5 ++++-
src/opendrive_road_segment.cpp | 36 ++++++++++++++++++++++----------
6 files changed, 37 insertions(+), 21 deletions(-)
diff --git a/include/opendrive_link.h b/include/opendrive_link.h
index f8ad897..fe015f0 100644
--- a/include/opendrive_link.h
+++ b/include/opendrive_link.h
@@ -36,7 +36,6 @@ class COpendriveLink
double get_resolution(void) const;
double get_scale_factor(void) const;
double find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point);
- double find_closest_local_point(TOpendriveLocalPoint &local,TPoint &point);
double get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
double get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index 90a2431..76dc362 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -31,6 +31,7 @@ class COpendriveRoadSegment
std::vector<COpendriveRoadSegment *> connecting;
std::string name;
unsigned int id;
+ opendrive_mark_t center_mark;
protected:
COpendriveRoadSegment();
COpendriveRoadSegment(const COpendriveRoadSegment &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoad *road_ref);
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 9251711..01e2542 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -296,12 +296,10 @@ TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
else
{
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)
- {
- track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
- }
}
if(this->get_id()>0)
world_point.heading=normalize_angle(world_point.heading+3.14159);
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 65b3771..6ded761 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -61,7 +61,6 @@ void COpendriveLink::generate(double start_curvature,double end_curvature,double
end.y=node_end.y;
end.heading=node_end.heading;
end.curvature=end_curvature;
-// std::cout << start.x << "," << start.y << "," << start.heading << "," << start_curvature << "," << end.x << "," << end.y << "," << end.heading << "," << end_curvature << std::endl;
this->spline=new CG2Spline(start,end);
this->spline->generate(this->resolution,length);
}
@@ -99,12 +98,14 @@ double COpendriveLink::get_scale_factor(void) const
double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point)
{
+ double length;
-}
-
-double COpendriveLink::find_closest_local_point(TOpendriveLocalPoint &local,TPoint &point)
-{
+ if(this->spline!=NULL)
+ length=this->spline->find_closest_point(world.x,world.y,point);
+ else
+ length=std::numeric_limits<double>::max();
+ return length;
}
double COpendriveLink::get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world)
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 5003ef5..7e21d0e 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -61,7 +61,10 @@ 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_start_point();
+ if(this->lanes[i]->get_id()<0)
+ lane_end=this->lanes[i]->get_end_point();
+ else
+ 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 fccf826..2b6ee40 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -252,7 +252,6 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
{
- opendrive_mark_t center_mark;
center::lane_type center_lane;
if(lane_section.center().lane().present())
@@ -261,30 +260,30 @@ void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_
if(center_lane.roadMark().size()>1)
throw CException(_HERE_,"Only one road mark supported for lane");
else if(center_lane.roadMark().size()==0)
- center_mark=OD_MARK_NONE;
+ this->center_mark=OD_MARK_NONE;
else if(center_lane.roadMark().size()==1)
{
if(center_lane.roadMark().begin()->type1().present())
{
if(center_lane.roadMark().begin()->type1().get()=="none")
- center_mark=OD_MARK_NONE;
+ this->center_mark=OD_MARK_NONE;
else if(center_lane.roadMark().begin()->type1().get()=="solid")
- center_mark=OD_MARK_SOLID;
+ this->center_mark=OD_MARK_SOLID;
else if(center_lane.roadMark().begin()->type1().get()=="broken")
- center_mark=OD_MARK_BROKEN;
+ this->center_mark=OD_MARK_BROKEN;
else if(center_lane.roadMark().begin()->type1().get()=="solid solid")
- center_mark=OD_MARK_SOLID_SOLID;
+ this->center_mark=OD_MARK_SOLID_SOLID;
else if(center_lane.roadMark().begin()->type1().get()=="solid broken")
- center_mark=OD_MARK_SOLID_BROKEN;
+ this->center_mark=OD_MARK_SOLID_BROKEN;
else if(center_lane.roadMark().begin()->type1().get()=="broken solid")
- center_mark=OD_MARK_BROKEN_SOLID;
+ this->center_mark=OD_MARK_BROKEN_SOLID;
else if(center_lane.roadMark().begin()->type1().get()=="broken broken")
- center_mark=OD_MARK_BROKEN_BROKEN;
+ this->center_mark=OD_MARK_BROKEN_BROKEN;
else
- center_mark=OD_MARK_NONE;
+ this->center_mark=OD_MARK_NONE;
}
else
- center_mark=OD_MARK_NONE;
+ this->center_mark=OD_MARK_NONE;
}
}
for(int i=-this->num_right_lanes;i<0;i++)
@@ -522,7 +521,22 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int
TOpendriveLocalPoint COpendriveRoadSegment::transform_to_local_point(const TOpendriveTrackPoint &track)
{
+ TOpendriveTrackPoint point;
+ TOpendriveLocalPoint local;
+
+ if(track.s<0.0)
+ throw CException(_HERE_,"Invalid track point");
+ point=track;
+ if(this->num_right_lanes>0)
+ local=this->lanes[-1]->transform_to_local_point(point);
+ else
+ {
+ point.s=this->lanes[1]->get_length()-track.s;
+ point.heading=normalize_angle(track.heading+3.14159);
+ local=this->lanes[1]->transform_to_local_point(point);
+ }
+ return local;
}
TOpendriveWorldPoint COpendriveRoadSegment::transform_to_world_point(const TOpendriveTrackPoint &track)
--
GitLab