From b44f59c738424648b52c22a663e2f3a36797fbe8 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 30 Dec 2020 15:56:24 +0100
Subject: [PATCH] Added functions to get the pose of signals and objects in
local and world coordinates.
---
include/opendrive_lane.h | 2 +
include/opendrive_road_segment.h | 6 ++-
src/opendrive_lane.cpp | 54 +++++++++++++++++++
src/opendrive_road_segment.cpp | 89 +++++++++++++++++++++++---------
4 files changed, 127 insertions(+), 24 deletions(-)
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index 8e461aa..7873c75 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -48,6 +48,8 @@ class COpendriveLane
TOpendriveWorldPoint get_start_point(void);
TOpendriveWorldPoint get_end_point(void);
double get_length(void);
+ TOpendriveLocalPoint transform_to_local_point(TOpendriveTrackPoint &track);
+ TOpendriveWorldPoint transform_to_world_point(TOpendriveTrackPoint &track);
friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
~COpendriveLane();
};
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index 51f15de..5834478 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -12,6 +12,8 @@
class COpendriveLane;
class COpendriveRoad;
+class COpendriveSignal;
+class COpendriveObject;
class COpendriveRoadSegment
{
@@ -53,10 +55,12 @@ class COpendriveRoadSegment
const COpendriveRoad &get_parent_road(void) const;
unsigned int get_num_signals(void) const;
const COpendriveSignal &get_signal(unsigned int index) const;
- unsigned int get_num_obstacles(void) const;
+ unsigned int get_num_objects(void) const;
const COpendriveObject &get_object(unsigned int index) const;
unsigned int get_num_connecting(void) const;
const COpendriveRoadSegment &get_connecting(unsigned int index) const;
+ TOpendriveLocalPoint transform_to_local_point(const TOpendriveTrackPoint &track);
+ TOpendriveWorldPoint transform_to_world_point(const TOpendriveTrackPoint &track);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
~COpendriveRoadSegment();
};
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 491758f..afa462d 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -309,6 +309,60 @@ double COpendriveLane::get_length(void)
return length;
}
+TOpendriveLocalPoint COpendriveLane::transform_to_local_point(TOpendriveTrackPoint &track)
+{
+ TOpendriveLocalPoint local;
+
+ if(track.s<0.0)
+ throw CException(_HERE_,"Invalid track point");
+ for(unsigned int i=0;i<this->nodes.size();i++)
+ {
+ for(unsigned int j=0;j<this->nodes[i]->get_num_lanes();j++)
+ {
+ if(this->nodes[i]->get_lane(j).get_id()==this->id)
+ {
+ if(track.s<=this->nodes[i]->get_geometry(j).get_length())
+ {
+ if(!this->nodes[i]->get_geometry(j).get_local_pose(track,local))
+ throw CException(_HERE_,"Impossible to transform to local coordinates");
+ return local;
+ }
+ else
+ track.s-=this->nodes[i]->get_geometry(j).get_length();
+ }
+ }
+ }
+
+ throw CException(_HERE_,"Track point does not belong to lane");
+}
+
+TOpendriveWorldPoint COpendriveLane::transform_to_world_point(TOpendriveTrackPoint &track)
+{
+ TOpendriveWorldPoint world;
+
+ if(track.s<0.0)
+ throw CException(_HERE_,"Invalid track point");
+ for(unsigned int i=0;i<this->nodes.size();i++)
+ {
+ for(unsigned int j=0;j<this->nodes[i]->get_num_lanes();j++)
+ {
+ if(this->nodes[i]->get_lane(j).get_id()==this->id)
+ {
+ if(track.s<=this->nodes[i]->get_geometry(j).get_length())
+ {
+ if(!this->nodes[i]->get_geometry(j).get_world_pose(track,world))
+ throw CException(_HERE_,"Impossible to transform to world coordinates");
+ return world;
+ }
+ else
+ track.s-=this->nodes[i]->get_geometry(j).get_length();
+ }
+ }
+ }
+
+ throw CException(_HERE_,"Track point does not belong to lane");
+}
+
std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
{
out << " Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 737342d..a1370fe 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -100,6 +100,10 @@ void COpendriveRoadSegment::set_scale_factor(double scale)
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
lane_it->second->set_scale_factor(scale);
+ for(unsigned int i=0;i<this->signals.size();i++)
+ this->signals[i]->set_scale_factor(scale);
+ for(unsigned int i=0;i<this->objects.size();i++)
+ this->objects[i]->set_scale_factor(scale);
}
void COpendriveRoadSegment::set_min_road_length(double length)
@@ -116,6 +120,10 @@ void COpendriveRoadSegment::update_references(std::map<COpendriveRoadSegment *,C
{
for(unsigned int i=0;i<this->connecting.size();i++)
this->connecting[i]=segment_refs[this->connecting[i]];
+ for(unsigned int i=0;i<this->signals.size();i++)
+ this->signals[i]->update_references(segment_refs);
+ for(unsigned int i=0;i<this->objects.size();i++)
+ this->objects[i]->update_references(segment_refs);
}
void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
@@ -123,6 +131,7 @@ void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
right::lane_iterator r_lane_it;
left::lane_iterator l_lane_it;
COpendriveLane *new_lane;
+ std::stringstream text;
// right lanes
if(lane_section.right().present())
@@ -137,7 +146,8 @@ void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
this->lanes[new_lane->get_id()]=new_lane;
this->num_right_lanes++;
}catch(CException &e){
- std::cout << e.what() << std::endl;
+ text << e.what() << " in road " << this->name;
+ throw CException(_HERE_,text.str());
}
}
}
@@ -154,7 +164,8 @@ void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
this->lanes[new_lane->get_id()]=new_lane;
this->num_left_lanes++;
}catch(CException &e){
- std::cout << e.what() << std::endl;
+ text << e.what() << " in road " << this->name;
+ throw CException(_HERE_,text.str());
}
}
}
@@ -397,30 +408,37 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
lane_section=road_info.lanes().laneSection().begin();
// add lanes
- this->add_lanes(*lane_section);
- // add road nodes
- this->add_nodes(road_info);
- // link lanes
- this->link_neightbour_lanes(*lane_section);
- // add road signals
- if(road_info.signals().present())
- {
- for(signal_it=road_info.signals().get().signal().begin();signal_it!=road_info.signals().get().signal().end();++signal_it)
+ try{
+ this->add_lanes(*lane_section);
+ // add road nodes
+ this->add_nodes(road_info);
+ // link lanes
+ this->link_neightbour_lanes(*lane_section);
+ // add road signals
+ if(road_info.signals().present())
{
- new_signal=new COpendriveSignal();
- new_signal->load(*signal_it);
- this->signals.push_back(new_signal);
+ for(signal_it=road_info.signals().get().signal().begin();signal_it!=road_info.signals().get().signal().end();++signal_it)
+ {
+ new_signal=new COpendriveSignal();
+ new_signal->load(*signal_it,this);
+ new_signal->set_scale_factor(this->scale_factor);
+ this->signals.push_back(new_signal);
+ }
}
- }
- // add road objects
- if(road_info.objects().present())
- {
- for(object_it=road_info.objects().get().object().begin();object_it!=road_info.objects().get().object().end();++object_it)
+ // add road objects
+ if(road_info.objects().present())
{
- new_object=new COpendriveObject();
- new_object->load(*object_it);
- this->objects.push_back(new_object);
+ for(object_it=road_info.objects().get().object().begin();object_it!=road_info.objects().get().object().end();++object_it)
+ {
+ new_object=new COpendriveObject();
+ new_object->load(*object_it,this);
+ new_object->set_scale_factor(this->scale_factor);
+ this->objects.push_back(new_object);
+ }
}
+ }catch(CException &e){
+ this->free();
+ throw e;
}
}
@@ -474,7 +492,7 @@ const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index) co
throw CException(_HERE_,"Invalid signal index");
}
-unsigned int COpendriveRoadSegment::get_num_obstacles(void) const
+unsigned int COpendriveRoadSegment::get_num_objects(void) const
{
return this->objects.size();
}
@@ -500,6 +518,31 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int
throw CException(_HERE_,"Invalid connecting segment index");
}
+TOpendriveLocalPoint COpendriveRoadSegment::transform_to_local_point(const TOpendriveTrackPoint &track)
+{
+
+}
+
+TOpendriveWorldPoint COpendriveRoadSegment::transform_to_world_point(const TOpendriveTrackPoint &track)
+{
+ TOpendriveTrackPoint point;
+ TOpendriveWorldPoint world;
+
+ if(track.s<0.0)
+ throw CException(_HERE_,"Invalid track point");
+ point=track;
+ if(this->num_right_lanes>0)
+ world=this->lanes[-1]->transform_to_world_point(point);
+ else
+ {
+ point.s=this->lanes[1]->get_length()-track.s;
+ point.heading=normalize_angle(track.heading+3.14159);
+ world=this->lanes[1]->transform_to_world_point(point);
+ }
+
+ return world;
+}
+
std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
{
out << "Road " << segment.get_name() << " (" << segment.get_id() << ")" << std::endl;
--
GitLab