opendrive_road_segment.cpp 26.5 KB
Newer Older
1
#include "opendrive_road_segment.h"
2
#include "exceptions.h"
3
#include <math.h>
4
5
6
7
8
9
10
11

COpendriveRoadSegment::COpendriveRoadSegment()
{
  this->name="";
  this->id=-1;
  this->lanes.clear();
  this->num_left_lanes=0;
  this->num_right_lanes=0;
12
  this->nodes.clear();
13
14
  this->signals.clear();
  this->objects.clear();
15
  this->connecting.clear();
16
17
18
  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
  this->resolution=DEFAULT_RESOLUTION;
  this->scale_factor=DEFAULT_SCALE_FACTOR;
19
20
}

21
COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object)
22
{
23
24
  COpendriveSignal *new_signal;
  COpendriveObject *new_object;
25
26
27

  this->name=object.name;
  this->id=object.id;
28
29
30
31
32
  this->lanes=object.lanes;
  this->num_right_lanes=object.num_right_lanes;
  this->num_left_lanes=object.num_left_lanes;
  this->nodes=object.nodes;
  this->parent_road=object.parent_road;
33
34
35
  this->signals.clear();
  for(unsigned int i=0;i<object.signals.size();i++)
  {
36
37
    new_signal=new COpendriveSignal(*object.signals[i]);
    this->signals.push_back(new_signal);
38
39
40
41
  }
  this->objects.clear();
  for(unsigned int i=0;i<object.objects.size();i++)
  {
42
43
    new_object=new COpendriveObject(*object.objects[i]);
    this->objects.push_back(new_object);
44
  }
45
  this->connecting=object.connecting;
46
47
48
  this->resolution=object.resolution;
  this->scale_factor=object.scale_factor;
  this->min_road_length=object.min_road_length;
49
  this->center_mark=object.center_mark;
50
51
}

52
53
54
55
56
57
58
59
60
61
void COpendriveRoadSegment::free(void)
{
  this->lanes.clear();
  this->num_left_lanes=0;
  this->num_right_lanes=0;
  for(unsigned int i=0;i<this->signals.size();i++)
  {
    delete this->signals[i];
    this->signals[i]=NULL;
  }
62
  this->nodes.clear();
63
64
65
66
67
68
69
  this->signals.clear();
  for(unsigned int i=0;i<this->objects.size();i++)
  {
    delete this->objects[i];
    this->objects[i]=NULL;
  }
  this->objects.clear();
70
  this->connecting.clear();
71
72
}

73
74
void COpendriveRoadSegment::set_resolution(double res)
{
75
76
77
78
79
80
  std::map<int,COpendriveLane *>::const_iterator lane_it;

  this->resolution=res;

  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
    lane_it->second->set_resolution(res);
81
82
83
84
}

void COpendriveRoadSegment::set_scale_factor(double scale)
{
85
86
87
88
89
90
  std::map<int,COpendriveLane *>::const_iterator lane_it;

  this->scale_factor=scale;

  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
    lane_it->second->set_scale_factor(scale);
91
92
93
94
  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);
95
96
97
98
99
}

void COpendriveRoadSegment::set_min_road_length(double length)
{
  this->min_road_length=length;
100
101
}

102
103
104
105
106
void COpendriveRoadSegment::set_parent_road(COpendriveRoad *parent)
{
  this->parent_road=parent;
}

107
void COpendriveRoadSegment::set_name(std::string &name)
108
{
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
  this->name=name;
}

void COpendriveRoadSegment::set_id(unsigned int id)
{
  this->id=id;
}

void COpendriveRoadSegment::set_center_mark(opendrive_mark_t mark)
{
  this->center_mark=mark;
}

bool COpendriveRoadSegment::updated(segment_up_ref_t &refs)
{
  segment_up_ref_t::iterator updated_it;

  for(updated_it=refs.begin();updated_it!=refs.end();updated_it++)
    if(updated_it->second==this)
      return true;

  return false;
}

void COpendriveRoadSegment::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
  std::vector<COpendriveRoadSegment *>::iterator seg_it;
  std::map<int,COpendriveLane *>::iterator lane_it;
  std::vector<COpendriveRoadNode *>::iterator node_it;

  if(this->updated(segment_refs))
  {
    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
    {
      if(lane_refs.find((*lane_it).second)!=lane_refs.end())
      {
        lane_it->second=lane_refs[lane_it->second];
        lane_it->second->update_references(segment_refs,lane_refs,node_refs);
      }
      else if(lane_it->second->updated(lane_refs))
        lane_it->second->update_references(segment_refs,lane_refs,node_refs);
    }
    for(node_it=this->nodes.begin();node_it!=this->nodes.end();node_it++)
      if(node_refs.find(*node_it)!=node_refs.end())
        (*node_it)=node_refs[*node_it];
    for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();seg_it++)
      if(segment_refs.find(*seg_it)!=segment_refs.end())
        (*seg_it)=segment_refs[*seg_it];
    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::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
  std::vector<COpendriveRoadSegment *>::iterator seg_it;
  std::map<int,COpendriveLane *>::iterator lane_it;
  std::vector<COpendriveRoadNode *>::iterator node_it;

  if(this->updated(segment_refs))
  {
    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
    {
      if(lane_refs.find((*lane_it).second)!=lane_refs.end())
      {
        lane_it->second=lane_refs[lane_it->second];
        lane_it->second->clean_references(segment_refs,lane_refs,node_refs);
        lane_it++;
      }
      else if(lane_it->second->updated(lane_refs))
      {
        lane_it->second->clean_references(segment_refs,lane_refs,node_refs);
        lane_it++;
      }
      else
        lane_it=this->lanes.erase(lane_it);
    }
    for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
    {
      if(node_refs.find(*node_it)!=node_refs.end())
      {
        (*node_it)=node_refs[*node_it];
        node_it++;
      }
      else if(!(*node_it)->updated(node_refs))
        node_it=this->nodes.erase(node_it);
      else
        node_it++;
    }
    for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();)
    {
      if(segment_refs.find(*seg_it)!=segment_refs.end())
      {
        (*seg_it)=segment_refs[*seg_it];
        seg_it++;
      }
      else if(!(*seg_it)->updated(segment_refs))
        seg_it=this->connecting.erase(seg_it);
      else
        seg_it++;
    }
  }
213
214
}

215
void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
216
217
218
219
{
  right::lane_iterator r_lane_it;
  left::lane_iterator l_lane_it;
  COpendriveLane *new_lane;
220
  std::stringstream text;
221
  unsigned int lane_index;
222
223

  // right lanes
224
  if(lane_section.right().present())
225
  {
226
    for(r_lane_it=lane_section.right()->lane().begin();r_lane_it!=lane_section.right()->lane().end();r_lane_it++)
227
228
229
    {
      try{
        new_lane=new COpendriveLane();
230
231
        new_lane->set_resolution(this->resolution);
        new_lane->set_scale_factor(this->scale_factor);
232
233
        new_lane->load(*r_lane_it,this);
        this->lanes[new_lane->get_id()]=new_lane;
234
235
        lane_index=parent_road->add_lane(new_lane);
        new_lane->set_index(lane_index);
236
        this->num_right_lanes++;
237
      }catch(CException &e){
238
239
        text << e.what() << " in road " << this->name;
        throw CException(_HERE_,text.str());
240
241
242
243
      }
    }
  }
  // left lanes
244
  if(lane_section.left().present())
245
  {
246
    for(l_lane_it=lane_section.left()->lane().begin();l_lane_it!=lane_section.left()->lane().end();l_lane_it++)
247
248
249
    {
      try{
        new_lane=new COpendriveLane();
250
251
        new_lane->set_resolution(this->resolution);
        new_lane->set_scale_factor(this->scale_factor);
252
253
        new_lane->load(*l_lane_it,this);
        this->lanes[new_lane->get_id()]=new_lane;
254
255
        lane_index=parent_road->add_lane(new_lane);
        new_lane->set_index(lane_index);
256
        this->num_left_lanes++;
257
      }catch(CException &e){
258
259
        text << e.what() << " in road " << this->name;
        throw CException(_HERE_,text.str());
260
261
262
      }
    }
  }
263
264
}

265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
void COpendriveRoadSegment::remove_lane(COpendriveLane *lane)
{
  std::map<int,COpendriveLane *>::iterator lane_it;
  std::vector<COpendriveLane *>::iterator other_lane_it;

  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
  {
    if(lane_it->second==lane)
    {
      if(lane_it->first>0)
        this->num_left_lanes--;
      else
        this->num_right_lanes--;
      // remove associated nodes
      for(unsigned int i=0;i<lane->nodes.size();i++)
        this->remove_node(lane->nodes[i]);
      // remove the reference from neightbour lanes
      if(lane_it->second->left_lane!=NULL)
        lane_it->second->left_lane->right_lane=NULL;
      if(lane_it->second->right_lane!=NULL)
        lane_it->second->right_lane->left_lane=NULL;
      // remove the reference from next lanes
      for(unsigned int i=0;i<lane_it->second->next.size();i++)
        for(other_lane_it=lane_it->second->next[i]->prev.begin();other_lane_it!=lane_it->second->next[i]->prev.end();)
        {
          if((*other_lane_it)==lane)
            other_lane_it=lane_it->second->next[i]->prev.erase(other_lane_it);
          else
            other_lane_it++;
        }
      // remove the reference from prev lanes
      for(unsigned int i=0;i<lane_it->second->prev.size();i++)
        for(other_lane_it=lane_it->second->prev[i]->next.begin();other_lane_it!=lane_it->second->prev[i]->next.end();)
        {
          if((*other_lane_it)==lane)
            other_lane_it=lane_it->second->prev[i]->next.erase(other_lane_it);
          else
            other_lane_it++;
        }
      lane_it=this->lanes.erase(lane_it);
      break;
    }
    else
      lane_it++;
  }
}

312
313
314
315
316
void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
{
  std::map<int,COpendriveLane *>::const_iterator lane_it;
  planView::geometry_iterator geom_it;
  COpendriveRoadNode *new_node;
317
  TOpendriveWorldPoint node_pose;
318
  double lane_offset;
319
  unsigned int index;
320

321
322
  try{
    for(geom_it=road_info.planView().geometry().begin(); geom_it!=road_info.planView().geometry().end(); ++geom_it)
323
    {
324
      lane_offset=0.0;
325
      for(int i=-1;i>=-this->num_right_lanes;i--)
326
327
328
329
330
331
      {
        new_node=new COpendriveRoadNode();
        new_node->set_resolution(this->resolution);
        new_node->set_scale_factor(this->scale_factor);
        this->lanes[i]->set_offset(lane_offset);
        new_node->load(*geom_it,this->lanes[i]);
332
        if(new_node->get_geometry(0).get_length()>this->min_road_length)
333
        {
334
335
336
337
338
339
340
341
          node_pose=new_node->get_start_pose();
          if(this->parent_road->node_exists_at(node_pose))
          {
            delete new_node;
            new_node=this->parent_road->get_node_at(node_pose);
            new_node->add_lane(*geom_it,this->lanes[i]);
          }
          else
342
343
344
345
          {
            index=this->parent_road->add_node(new_node);
            new_node->set_index(index);
          }
346
347
          new_node->add_parent_segment(this);
          this->add_node(new_node);
348
349
          this->lanes[i]->add_node(new_node);
        }
350
        lane_offset-=this->lanes[i]->width;
351
      }
352
353
354
355
    }
    for(geom_it=road_info.planView().geometry().end(); geom_it!=road_info.planView().geometry().begin();)
    {
      geom_it--;
356
      lane_offset=0.0;
357
      for(int i=1;i<=this->num_left_lanes;i++)
358
359
360
361
362
363
      {
        new_node=new COpendriveRoadNode();
        new_node->set_resolution(this->resolution);
        new_node->set_scale_factor(this->scale_factor);
        this->lanes[i]->set_offset(lane_offset);
        new_node->load(*geom_it,this->lanes[i]);
364
        if(new_node->get_geometry(0).get_length()>this->min_road_length)
365
        {
366
367
368
369
370
371
372
373
          node_pose=new_node->get_start_pose();
          if(this->parent_road->node_exists_at(node_pose))
          {
            delete new_node;
            new_node=this->parent_road->get_node_at(node_pose);
            new_node->add_lane(*geom_it,this->lanes[i]);
          }
          else
374
375
376
377
          {
            index=this->parent_road->add_node(new_node);
            new_node->set_index(index);
          }
378
379
          new_node->add_parent_segment(this);
          this->add_node(new_node);
380
381
          this->lanes[i]->add_node(new_node);
        }
382
        lane_offset+=this->lanes[i]->width;
383
      }
384
    }
385
386
387
388
389
  }catch(CException &e){
    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
      delete lane_it->second;
    this->lanes.clear();
    throw e;
390
391
392
  }
}

393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
void COpendriveRoadSegment::add_node(COpendriveRoadNode *node)
{
  for(unsigned int i=0;i<this->nodes.size();i++)
    if(this->nodes[i]==node)
      return;
  //add the node
  this->nodes.push_back(node);
}

void COpendriveRoadSegment::remove_node(COpendriveRoadNode *node)
{
  std::vector<COpendriveRoadNode *>::iterator it;

  for(it=this->nodes.begin();it!=this->nodes.end();)
  {
    if((*it)==node)
    {
      it=this->nodes.erase(it);
      break;
    }
    else
      it++;
  }
}

bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node)
{
  for(unsigned int i=0;i<this->nodes.size();i++)
    if(this->nodes[i]==node)
      return true;

  return false;
}

int COpendriveRoadSegment::get_node_side(COpendriveRoadNode *node)
{
  std::map<int,COpendriveLane *>::iterator it;

  if(this->has_node(node))
  {
    for(it=this->lanes.begin();it!=this->lanes.end();it++)
      if(it->second->has_node(node))
        return it->first;

    return 0;
  }
  else
    throw CException(_HERE_,"Road segment does not include the given node");
}

443
void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
444
445
446
447
448
449
{
  center::lane_type center_lane;

  if(lane_section.center().lane().present())
  {
    center_lane=lane_section.center().lane().get();
450
451
452
    if(center_lane.roadMark().size()>1)
      throw CException(_HERE_,"Only one road mark supported for lane");
    else if(center_lane.roadMark().size()==0)
453
      this->set_center_mark(OD_MARK_NONE);
454
    else if(center_lane.roadMark().size()==1)
455
    {
456
457
458
      if(center_lane.roadMark().begin()->type1().present())
      {
        if(center_lane.roadMark().begin()->type1().get()=="none")
459
          this->set_center_mark(OD_MARK_NONE);
460
        else if(center_lane.roadMark().begin()->type1().get()=="solid")
461
          this->set_center_mark(OD_MARK_SOLID);
462
        else if(center_lane.roadMark().begin()->type1().get()=="broken")
463
          this->set_center_mark(OD_MARK_BROKEN);
464
        else if(center_lane.roadMark().begin()->type1().get()=="solid solid")
465
          this->set_center_mark(OD_MARK_SOLID_SOLID);
466
        else if(center_lane.roadMark().begin()->type1().get()=="solid broken")
467
          this->set_center_mark(OD_MARK_SOLID_BROKEN);
468
        else if(center_lane.roadMark().begin()->type1().get()=="broken solid")
469
          this->set_center_mark(OD_MARK_BROKEN_SOLID);
470
        else if(center_lane.roadMark().begin()->type1().get()=="broken broken")
471
          this->set_center_mark(OD_MARK_BROKEN_BROKEN);
472
        else
473
          this->set_center_mark(OD_MARK_NONE);
474
      }
475
      else
476
        this->set_center_mark(OD_MARK_NONE);
477
    }
478
479
480
481
482
  }
  for(int i=-this->num_right_lanes;i<0;i++)
  {
    if(this->lanes.find(i+1)!=this->lanes.end())// if the lane exists 
      this->lanes[i]->link_neightbour_lane(this->lanes[i+1]);
483
    else
484
      this->lanes[i]->left_mark=OD_MARK_NONE;
485
  }
486
487
488
489
490
491
492
  for(int i=this->num_left_lanes;i>0;i--)
  {
    if(this->lanes.find(i-1)!=this->lanes.end())// if the lane exists 
      this->lanes[i]->link_neightbour_lane(this->lanes[i-1]);
    else
      this->lanes[i]->right_mark=OD_MARK_NONE;
  }
493
494
495
496
497
498

  if(this->lanes.find(1)!=this->lanes.end())
    this->lanes[1]->right_mark=this->center_mark;
  if(this->lanes.find(-1)!=this->lanes.end())
    this->lanes[-1]->left_mark=this->center_mark;
//  this->lanes[-1]->link_neightbour_lane(this->lanes[1]);
499
500
501
502
}

void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment)
{
503
504
  for(unsigned int i=0;i<this->connecting.size();i++)
    if(this->connecting[i]->get_id()==segment.get_id())// the segment is already included
505
      return;
506
507
  for(unsigned int i=0;i<segment.connecting.size();i++)
    if(segment.connecting[i]->get_id()==this->id)
508
      return;
509
510
  this->connecting.push_back(&segment);
  segment.connecting.push_back(this);
511
512
  // link lanes
  for(int i=-this->num_right_lanes;i<0;i++)
513
  {
514
    for(int j=i-1;j<=i+1;j++)
515
    {
516
      if(segment.lanes.find(j)!=segment.lanes.end())
517
      {
518
        if(j==i-1)
519
          this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->right_mark,false,true);
520
        else if(j==i)
521
          this->lanes[i]->link_lane(segment.lanes[j],OD_MARK_NONE,false,true);
522
        else
523
          this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->left_mark,false,true);
524
525
526
      }
    }
  }
527
  for(int i=1;i<=segment.num_left_lanes;i++)
528
  {
529
    for(int j=i-1;j<=i+1;j++)
530
    {
531
      if(this->lanes.find(j)!=this->lanes.end())
532
      {
533
        if(j==i-1)
534
          segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->right_mark,false,true);
535
        else if(j==i)
536
          segment.lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true);
537
        else
538
          segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->left_mark,false,true);
539
540
541
542
543
      }
    }
  }
}

544
void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from,bool from_start,int to,bool to_start)
545
{
546
547
  bool connect=true;

548
  for(unsigned int i=0;i<this->connecting.size();i++)
549
  {
550
    if(this->connecting[i]->get_id()==segment.get_id())// the segment is already included
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
    {
      for(unsigned int j=0;j<segment.connecting.size();j++)
      {
        if(segment.connecting[j]->get_id()==this->id)
        {
          connect=false;
          break;
        }
      }
    }
  }
  if(connect)
  {
    this->connecting.push_back(&segment);
    segment.connecting.push_back(this);
  }
567
  // link lanes
568
569
570
  if(segment.lanes.find(to)!=segment.lanes.end())
  {
    if(this->lanes.find(from-1)!=this->lanes.end())
571
      this->lanes[from-1]->link_lane(segment.lanes[to],this->lanes[from-1]->right_mark,from_start,to_start);
572
    if(this->lanes.find(from)!=this->lanes.end())
573
      this->lanes[from]->link_lane(segment.lanes[to],OD_MARK_NONE,from_start,to_start);
574
    if(this->lanes.find(from+1)!=this->lanes.end())
575
      this->lanes[from+1]->link_lane(segment.lanes[to],this->lanes[from+1]->left_mark,from_start,to_start);
576
  }
577
578
}

579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment)
{
  for(unsigned int i=0;i<this->connecting.size();i++)
    if(this->connecting[i]==segment)
      return true;

  return false;
}

bool COpendriveRoadSegment::connects_nodes(COpendriveRoadNode *node1,COpendriveRoadNode *node2)
{
  if(this->has_node(node1) && this->has_node(node2))
    return true;
  else
    return false;
}

COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPoint *start,TOpendriveWorldPoint *end)
{
  std::map<int,COpendriveLane *>::iterator lane_it;
  lane_up_ref_t::iterator lane_ref_it;
  COpendriveLane *new_lane;
  std::vector<COpendriveRoadSegment *>::iterator seg_it;
  segment_up_ref_t new_segment_ref;
  COpendriveRoadSegment *new_segment;
  std::vector<COpendriveRoadNode *>::iterator node_it;
  node_up_ref_t::iterator node_ref_it;
  COpendriveRoadNode *new_node;

  if(start==NULL && end==NULL)
    return this->clone(new_node_ref,new_lane_ref);
  new_segment=new COpendriveRoadSegment(*this);
  new_segment_ref[this]=new_segment;
  /* get the sublanes */
  for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
  {
    if(node_side<0)
      new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,&new_node,start,end);
    else
      new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,&new_node,end,start);
    new_lane_ref[lane_it->second]=new_lane;
    if(new_node!=NULL)
      new_segment->add_node(new_node);
  }
  new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
  // update signals and objects

  return new_segment; 
}

COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref)
{
  std::map<int,COpendriveLane *>::iterator lane_it;
  lane_up_ref_t::iterator lane_ref_it;
  COpendriveLane *new_lane;
  segment_up_ref_t new_segment_ref;
  COpendriveRoadSegment *new_segment;
  std::vector<COpendriveRoadNode *>::iterator node_it;
  node_up_ref_t::iterator node_ref_it;

  new_segment=new COpendriveRoadSegment(*this);
  new_segment_ref[this]=new_segment;
  /* get the sublanes */
  for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
  {
    new_lane=lane_it->second->clone(new_node_ref,new_lane_ref);
    new_lane_ref[lane_it->second]=new_lane;
  }
  new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);

  return new_segment;
}

652
653
654
655
656
657
658
659
660
661
void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
{
  std::map<int,COpendriveLane *>::const_iterator lane_it;
  signals::signal_iterator signal_it;
  objects::object_iterator object_it;
  lanes::laneSection_iterator lane_section;
  COpendriveSignal *new_signal;
  COpendriveObject *new_object;

  this->free();
662
663
  this->set_name(road_info.name().get());
  this->set_id(std::stoi(road_info.id().get()));
664
665
666
667
668
669
670
671
672
  // only one lane section is supported
  if(road_info.lanes().laneSection().size()<1)
    throw CException(_HERE_,"No lane sections defined for road "+road_info.id().get());
  else if(road_info.lanes().laneSection().size()>1)
    throw CException(_HERE_,"Road "+road_info.id().get()+" has more than one lane section");
  else
    lane_section=road_info.lanes().laneSection().begin();

  // add lanes
673
674
675
676
677
678
679
680
  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())
681
    {
682
683
684
685
686
687
688
      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);
      }
689
    }
690
691
    // add road objects
    if(road_info.objects().present())
692
    {
693
694
695
696
697
698
699
      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);
      }
700
    }
701
702
703
  }catch(CException &e){
    this->free();
    throw e;
704
705
706
  }
}

707
std::string COpendriveRoadSegment::get_name(void) const
708
709
710
711
{
  return this->name;
}

712
unsigned int COpendriveRoadSegment::get_id(void) const
713
714
715
716
{
  return this->id;
}

717
unsigned int COpendriveRoadSegment::get_num_right_lanes(void) const
718
719
720
721
{
  return this->num_right_lanes;
}

722
unsigned int COpendriveRoadSegment::get_num_left_lanes(void) const
723
724
725
726
{
  return this->num_left_lanes;
}

727
const COpendriveLane &COpendriveRoadSegment::get_lane(int index) const
728
{
729
730
731
732
733
734
735
736
  std::map<int,COpendriveLane *>::const_iterator lane_it;

  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
  {
    if(lane_it->second->get_id()==index)
      return *lane_it->second;
  }
  throw CException(_HERE_,"invalid lane index");
737
738
}

739
740
741
742
743
744
745
746
747
748
749
750
751
unsigned int COpendriveRoadSegment::get_num_nodes(void) const
{
  return this->nodes.size();
}

const COpendriveRoadNode &COpendriveRoadSegment::get_node(unsigned int index) const
{
  if(index>=0 && index<this->nodes.size())
    return *this->nodes[index];
  else
    throw CException(_HERE_,"Invalid signal index");
}

752
753
754
755
756
const COpendriveRoad &COpendriveRoadSegment::get_parent_road(void) const
{
  return *this->parent_road;
}

757
unsigned int COpendriveRoadSegment::get_num_signals(void) const
758
759
760
761
{
  return this->signals.size();  
}

762
const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index) const
763
764
765
766
767
768
769
{
  if(index>=0 && index<this->signals.size())
    return *this->signals[index];
  else
    throw CException(_HERE_,"Invalid signal index");
}

770
unsigned int COpendriveRoadSegment::get_num_objects(void) const
771
772
773
774
{
  return this->objects.size();
}

775
const COpendriveObject &COpendriveRoadSegment::get_object(unsigned int index) const
776
777
778
779
780
781
782
{
  if(index>=0 && index<this->objects.size())
    return *this->objects[index];
  else
    throw CException(_HERE_,"Invalid object index");
}

783
unsigned int COpendriveRoadSegment::get_num_connecting(void) const
784
{
785
  return this->connecting.size();
786
787
}

788
const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int index) const
789
{
790
791
  if(index>=0 && index<this->connecting.size())
    return *this->connecting[index];
792
  else
793
    throw CException(_HERE_,"Invalid connecting segment index");
794
795
}

796
797
TOpendriveLocalPoint COpendriveRoadSegment::transform_to_local_point(const TOpendriveTrackPoint &track)
{
798
799
800
801
802
803
804
805
806
807
808
809
810
811
  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);
  }
812

813
  return local;
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
}

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

836
std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
837
{
838
  out << "Road " << segment.get_name() << " (" << segment.get_id() << ")" << std::endl;
839
840
841
  out << "  Connecting road segments: " << segment.connecting.size() << std::endl;
  for(unsigned int i=0;i<segment.connecting.size();i++)
    out << "    " << segment.connecting[i]->get_name() << std::endl;
842
843
844
  out << "  Number of nodes: " << segment.get_num_nodes() << std::endl;
  for(unsigned int i=0;i<segment.get_num_nodes();i++)
    out << "    " << segment.get_node(i).get_index() << std::endl;
845
  out << "  Number of right lanes: " << segment.num_right_lanes << std::endl;
846
847
  for(int i=-1;i>=-segment.num_right_lanes;i--)
    out << *segment.lanes[i];
848
  out << "  Number of left lanes: " << segment.num_left_lanes << std::endl;
849
850
  for(int i=1;i<=segment.num_left_lanes;i++)
    out << *segment.lanes[i];
851
852
  out << "  Number of signals: " << segment.signals.size() << std::endl;
  for(unsigned int i=0;i<segment.signals.size();i++)
853
    std::cout << *segment.signals[i];
854
855
  out << "  Number of objects: " << segment.objects.size() << std::endl;
  for(unsigned int i=0;i<segment.objects.size();i++)
856
    std::cout << *segment.objects[i];
857

858
  return out;
859
860
861
862
}

COpendriveRoadSegment::~COpendriveRoadSegment()
{
863
  this->free();
864
865
  this->name="";
  this->id=-1;
866
867
868
  this->resolution=DEFAULT_RESOLUTION;
  this->scale_factor=DEFAULT_SCALE_FACTOR;
  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
869
870
}