Added functions to get the pose of signals and objects in local and world coordinates.

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();
 };

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();
 };

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;

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;