opendrive_lane.cpp 25.1 KB
Newer Older
1
#include "opendrive_lane.h"
2
#include "exceptions.h"
3
#include <math.h>
4
5
6
7

COpendriveLane::COpendriveLane()
{
  this->nodes.clear();
8
9
  this->next.clear();
  this->prev.clear();
10
11
12
13
  this->left_lane=NULL;
  this->left_mark=OD_MARK_NONE;
  this->right_lane=NULL;
  this->right_mark=OD_MARK_NONE;
14
  this->segment=NULL;
15
  this->resolution=DEFAULT_RESOLUTION;
16
17
18
19
  this->scale_factor=DEFAULT_SCALE_FACTOR;
  this->width=0.0;
  this->speed=0.0;
  this->offset=0.0;
20
  this->id=0;
21
  this->index=-1;
22
23
}

24
COpendriveLane::COpendriveLane(const COpendriveLane &object)
25
{
26
27
28
  this->nodes=object.nodes;
  this->next=object.next;
  this->prev=object.prev;
29
30
31
32
  this->left_lane=object.left_lane;
  this->left_mark=object.left_mark;
  this->right_lane=object.right_lane;
  this->right_mark=object.right_mark;
33
  this->segment=object.segment;
34
35
  this->resolution=object.resolution;
  this->scale_factor=object.scale_factor;
36
37
38
  this->width=object.width;
  this->speed=object.speed;
  this->offset=object.offset;
39
  this->id=object.id;
40
  this->index=object.index;
41
42
}

43
void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
44
{
Sergi Hernandez's avatar
Sergi Hernandez committed
45
46
  unsigned int min_num_nodes;

47
48
  if(lane!=NULL)
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
49
50
51
52
53
    if(this->get_num_nodes()<lane->get_num_nodes())
      min_num_nodes=this->get_num_nodes();
    else
      min_num_nodes=lane->get_num_nodes();
    for(unsigned int i=0;i<min_num_nodes-1;i++)
54
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
55
56
57
      this->nodes[i]->add_link(lane->nodes[i+1],lane->right_mark,this->segment,NULL);
      lane->nodes[i]->add_link(this->nodes[i+1],lane->right_mark,this->segment,NULL);
      this->left_mark=lane->right_mark;
58
    } 
Sergi Hernandez's avatar
Sergi Hernandez committed
59
60
61
62
63
    if(min_num_nodes>0)
    {
      this->left_lane=lane;
      lane->right_lane=this;
    }
64
65
66
  }
}

Sergi Hernandez's avatar
Sergi Hernandez committed
67
void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start, bool to_start,bool belongs_to_lane)
68
{
69
  COpendriveRoadNode *start,*end;
Sergi Hernandez's avatar
Sergi Hernandez committed
70
  std::stringstream error;
71

72
73
  if(lane!=NULL)
  {
74
    if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
    {
      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];
        }
      }
Sergi Hernandez's avatar
Sergi Hernandez committed
118
119
120
121
      if(belongs_to_lane)
        start->add_link(end,mark,this->segment,this);
      else
        start->add_link(end,mark,this->segment,NULL);
122
123
124
      // link lane
      this->add_next_lane(lane);
      lane->add_prev_lane(this);
125
    }
126
    else 
Sergi Hernandez's avatar
Sergi Hernandez committed
127
128
129
130
    {
      error << "Lane " << this->id << " of segment " << this->segment->get_name() << " or lane " << lane->get_id() << " of segment " << lane->get_segment().get_name() << " has no nodes";
      throw CException(_HERE_,error.str());
    }
131
132
133
  }
}

134
void COpendriveLane::add_next_lane(COpendriveLane *lane)
135
{
136
137
  for(unsigned int i=0;i<this->next.size();i++)
    if(this->next[i]==lane)// lane is already linked
138
      return;
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153

  // add the lane
  this->next.push_back(lane);
}

void COpendriveLane::add_prev_lane(COpendriveLane *lane)
{
  for(unsigned int i=0;i<this->prev.size();i++)
    if(this->prev[i]==lane)// lane is already linked
      return;

  // add the lane
  this->prev.push_back(lane);
}

Sergi Hernandez's avatar
Sergi Hernandez committed
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
void COpendriveLane::remove_lane(COpendriveLane *lane)
{
  std::vector<COpendriveLane *>::iterator lane_it;

  // remove the reference from neightbour lanes
  if(this->right_lane==lane)
    this->right_lane=NULL;
  if(this->left_lane==lane)
    this->left_lane=NULL;
  // remove the reference from next lanes
  for(lane_it=this->next.begin();lane_it!=this->next.end();)
  {
    if((*lane_it)==lane)
      lane_it=this->next.erase(lane_it);
     else
      lane_it++;
  }
  // remove the reference from next lanes
  for(lane_it=this->prev.begin();lane_it!=this->prev.end();)
  {
    if((*lane_it)==lane)
      lane_it=this->prev.erase(lane_it);
     else
      lane_it++;
  }
}

181
182
183
184
void COpendriveLane::add_node(COpendriveRoadNode *node)
{
  if(this->has_node(node))
    return;
Sergi Hernandez's avatar
Sergi Hernandez committed
185
186
  // link the new node with the previous one in the current lane, if any
  if(this->nodes.size()>0)
187
    this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE,this->segment,this);
188
189
  // add the new node
  this->nodes.push_back(node);
190
191
}

192
193
194
195
196
197
198
199
200
bool COpendriveLane::has_node(COpendriveRoadNode *node)
{
  for(unsigned int i=0;i<this->nodes.size();i++)
    if(this->nodes[i]==node)
      return true;

  return false;
}

Sergi Hernandez's avatar
Sergi Hernandez committed
201
bool COpendriveLane::has_node_with_index(unsigned int index)
202
203
204
205
206
207
208
209
210
211
212
213
214
{
  for(unsigned int i=0;i<this->nodes.size();i++)
    if(this->nodes[i]->get_index()==index)
      return true;

  return false;
}

void COpendriveLane::set_parent_segment(COpendriveRoadSegment *segment)
{
  this->segment=segment;
}

Sergi Hernandez's avatar
Sergi Hernandez committed
215
216
217
218
219
220
221
222
223
224
225
226
void COpendriveLane::set_left_lane(COpendriveLane *lane,opendrive_mark_t mark)
{
  this->left_lane=lane;
  this->left_mark=mark;
}

void COpendriveLane::set_right_lane(COpendriveLane *lane,opendrive_mark_t mark)
{
  this->right_lane=lane;
  this->right_mark=mark;
}

227
void COpendriveLane::set_resolution(double res)
228
229
230
{
  this->resolution=res;

231
232
233
234
235
236
237
238
239
240
241
242
  for(unsigned int i=0;i<this->nodes.size();i++)
    this->nodes[i]->set_resolution(res);
}

void COpendriveLane::set_scale_factor(double scale)
{ 
  this->scale_factor=scale;

  for(unsigned int i=0;i<this->nodes.size();i++)
    this->nodes[i]->set_scale_factor(scale);
}

243
244
245
246
247
248
249
250
251
252
void COpendriveLane::set_width(double width)
{
  this->width=width;
}

void COpendriveLane::set_speed(double speed)
{
  this->speed=speed;
}

253
254
255
256
257
void COpendriveLane::set_offset(double offset)
{
  this->offset=offset;
}

258
void COpendriveLane::set_id(int id)
259
{
260
261
  this->id=id;
}
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
312
313
314
315
316
317
318
319
void COpendriveLane::set_index(unsigned int index)
{
  this->index=index;
}

bool COpendriveLane::updated(lane_up_ref_t &refs)
{
  lane_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 COpendriveLane::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
  std::vector<COpendriveLane *>::iterator lane_it;
  std::vector<COpendriveRoadNode *>::iterator node_it;
 
  if(this->updated(lane_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];
        (*node_it)->update_references(segment_refs,lane_refs,node_refs);
      }
      else if((*node_it)->updated(node_refs))
        (*node_it)->update_references(segment_refs,lane_refs,node_refs);
    }
    for(lane_it=this->next.begin();lane_it!=this->next.end();lane_it++)
      if(lane_refs.find(*lane_it)!=lane_refs.end())
        (*lane_it)=lane_refs[*lane_it];
    for(lane_it=this->prev.begin();lane_it!=this->prev.end();lane_it++)
      if(lane_refs.find(*lane_it)!=lane_refs.end())
        (*lane_it)=lane_refs[*lane_it];
    if(lane_refs.find(this->left_lane)!=lane_refs.end())
      this->left_lane=lane_refs[this->left_lane];
    if(lane_refs.find(this->right_lane)!=lane_refs.end())
      this->right_lane=lane_refs[this->right_lane];
    if(segment_refs.find(this->segment)!=segment_refs.end())
      this->segment=segment_refs[this->segment];
  }
}

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

  if(this->updated(lane_refs))
  {
    for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
320
      if((*node_it)->updated(node_refs))
321
322
323
324
325
326
327
328
329
      {
        (*node_it)->clean_references(segment_refs,lane_refs,node_refs);
        node_it++;
      }
      else
        node_it=this->nodes.erase(node_it);
    }
    for(lane_it=this->next.begin();lane_it!=this->next.end();)
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
330
      if(!(*lane_it)->updated(lane_refs))
331
332
333
334
335
336
        lane_it=this->next.erase(lane_it);
      else
        lane_it++;
    }
    for(lane_it=this->prev.begin();lane_it!=this->prev.end();)
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
337
      if(!(*lane_it)->updated(lane_refs))
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
        lane_it=this->prev.erase(lane_it);
      else
        lane_it++;
    }
    if(lane_refs.find(this->left_lane)!=lane_refs.end())
      this->left_lane=lane_refs[this->left_lane];
    else if(!this->left_lane->updated(lane_refs))
      this->left_lane=NULL;
    if(lane_refs.find(this->right_lane)!=lane_refs.end())
      this->right_lane=lane_refs[this->right_lane];
    else if(!this->right_lane->updated(lane_refs))
      this->right_lane=NULL;
  }
}

Sergi Hernandez's avatar
Sergi Hernandez committed
353
void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start)
354
355
356
357
{
  std::vector<COpendriveRoadNode *>::iterator it;
  segment_up_ref_t new_segment_ref;
  unsigned int start_node_index,i;
Sergi Hernandez's avatar
Sergi Hernandez committed
358
  TOpendriveWorldPose start_pose;
359
  COpendriveRoadNode *new_node;
Sergi Hernandez's avatar
Sergi Hernandez committed
360
  std::stringstream error;
Sergi Hernandez's avatar
Sergi Hernandez committed
361
  double distance;
362
363
364
  
  if(start==NULL)
    return;
Sergi Hernandez's avatar
Sergi Hernandez committed
365
  start_node_index=this->get_closest_node_index(*start,distance,3.14159);
Sergi Hernandez's avatar
Sergi Hernandez committed
366
367
368
369
370
  if(start_node_index==(unsigned int)-1)
  {
    error << "No node close to (x=" << start->x << ",y=" << start->y << ",heading=" << start->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
    throw CException(_HERE_,error.str());
  }
Sergi Hernandez's avatar
Sergi Hernandez committed
371
372
  // get the actual start position on the lane
  this->get_closest_pose(*start,start_pose,3.14159);
373
374
375
376
  // eliminate all the node before the start one
  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
  {
    if(i<start_node_index)
377
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
378
379
380
381
382
      if((*it)->updated(new_node_ref))
      {
        new_node_ref.erase((*it)->get_original_node(new_node_ref));
        delete *it;
      }
383
      it=this->nodes.erase(it);
384
    }
385
386
    else
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
387
      if(!(*it)->updated(new_node_ref))
388
      {
Sergi Hernandez's avatar
Sergi Hernandez committed
389
        new_node=(*it)->clone(new_link_ref);
390
391
392
393
394
395
396
        new_node_ref[*it]=new_node;
      }
      it++;
    }
  }
  this->prev.clear();// it is no longer connected to any previous lane
  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
Sergi Hernandez's avatar
Sergi Hernandez committed
397
  this->nodes[0]->update_start_pose(this,start_pose,distance);
398
399
}

Sergi Hernandez's avatar
Sergi Hernandez committed
400
void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *end)
401
402
403
404
{
  std::vector<COpendriveRoadNode *>::iterator it;
  segment_up_ref_t new_segment_ref;
  unsigned int end_node_index,i;
Sergi Hernandez's avatar
Sergi Hernandez committed
405
  TOpendriveWorldPose end_pose;
406
  COpendriveRoadNode *new_node;
Sergi Hernandez's avatar
Sergi Hernandez committed
407
  std::stringstream error;
Sergi Hernandez's avatar
Sergi Hernandez committed
408
  double distance;
409
410
    
  if(end==NULL)
411
    return;
Sergi Hernandez's avatar
Sergi Hernandez committed
412
  end_node_index=this->get_closest_node_index(*end,distance,3.14159);
Sergi Hernandez's avatar
Sergi Hernandez committed
413
414
415
416
417
  if(end_node_index==(unsigned int)-1)
  {
    error << "No node close to (x=" << end->x << ",y=" << end->y << ",heading=" << end->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
    throw CException(_HERE_,error.str());
  }
Sergi Hernandez's avatar
Sergi Hernandez committed
418
  this->get_closest_pose(*end,end_pose,3.14159);
419
420
421
422
  // eliminate all the node before the start one
  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
  {
    if(i>end_node_index)
423
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
424
425
426
427
428
      if((*it)->updated(new_node_ref))
      {
        new_node_ref.erase((*it)->get_original_node(new_node_ref));
        delete *it;
      }
429
      it=this->nodes.erase(it);
430
    }
431
432
    else
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
433
      if(!(*it)->updated(new_node_ref))
434
      {
Sergi Hernandez's avatar
Sergi Hernandez committed
435
        new_node=(*it)->clone(new_link_ref);
436
437
438
439
440
441
442
        new_node_ref[*it]=new_node;
      }
      it++;
    }
  }
  this->next.clear();// it is no longer connected to any next lane
  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
Sergi Hernandez's avatar
Sergi Hernandez committed
443
  this->nodes[this->nodes.size()-1]->update_end_pose(this,end_pose,distance);
444
445
}

Sergi Hernandez's avatar
Sergi Hernandez committed
446
COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
447
448
449
450
{
  COpendriveLane *new_lane;

  if(start==NULL && end==NULL)
Sergi Hernandez's avatar
Sergi Hernandez committed
451
    return this->clone(new_node_ref,new_lane_ref,new_link_ref);
452
453
454
455
  new_lane=new COpendriveLane(*this);
  new_lane_ref[this]=new_lane;
  if(this->id<0)
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
456
457
    new_lane->update_start_node(new_node_ref,new_lane_ref,new_link_ref,start);
    new_lane->update_end_node(new_node_ref,new_lane_ref,new_link_ref,end);
458
459
460
  }
  else
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
461
462
    new_lane->update_start_node(new_node_ref,new_lane_ref,new_link_ref,end);
    new_lane->update_end_node(new_node_ref,new_lane_ref,new_link_ref,start);
463
464
465
466
467
  }

  return new_lane;
}

Sergi Hernandez's avatar
Sergi Hernandez committed
468
COpendriveLane *COpendriveLane::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref)
469
470
471
472
473
474
475
476
477
478
{
  COpendriveLane *new_lane;
  std::vector<COpendriveRoadNode *>::iterator it;
  COpendriveRoadNode *new_node;
  segment_up_ref_t new_segment_ref;

  new_lane=new COpendriveLane(*this);
  new_lane_ref[this]=new_lane;
  for(it=new_lane->nodes.begin();it!=new_lane->nodes.end();it++)
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
479
    new_node=(*it)->clone(new_link_ref);
480
481
482
483
484
    new_node_ref[*it]=new_node;
  }
  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);

  return new_lane;
485
486
}

Sergi Hernandez's avatar
Sergi Hernandez committed
487
void COpendriveLane::load(const ::lane &lane_info,COpendriveRoadSegment *segment)
488
489
{
  std::stringstream error;
490
  opendrive_mark_t mark;
491
492

  this->nodes.clear();
Sergi Hernandez's avatar
Sergi Hernandez committed
493
494
  this->prev.clear();
  this->next.clear();
495
496
497
498
  this->left_lane=NULL;
  this->left_mark=OD_MARK_NONE;
  this->right_lane=NULL;
  this->right_mark=OD_MARK_NONE;
499
  this->set_id(lane_info.id().get());
500
  // import lane width
Sergi Hernandez's avatar
Sergi Hernandez committed
501
502
503
504
505
506
  if(lane_info.width().size()<1)
  {
    error << "Lane " << this->id << " of segment " << segment->get_name() << " has no width record";
    throw CException(_HERE_,error.str());
  }
  else if(lane_info.width().size()>1)
507
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
508
    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one width record";
509
510
    throw CException(_HERE_,error.str());
  }
511
  this->set_width(lane_info.width().begin()->a().get());
512
  // import lane speed
Sergi Hernandez's avatar
Sergi Hernandez committed
513
  if(lane_info.speed().size()<1)
514
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
515
516
517
518
519
520
    error << "Lane " << this->id << " of segment " << segment->get_name() << " has no speed record";
    throw CException(_HERE_,error.str());
  }
  else if(lane_info.speed().size()>1)
  {
    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one speed record";
521
522
    throw CException(_HERE_,error.str());
  }
523
  this->set_speed(lane_info.speed().begin()->max().get());
524
  //lane mark
525
526
527
  if(lane_info.roadMark().size()==0)
    mark=OD_MARK_NONE;
  else if(lane_info.roadMark().size()>1)
528
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
529
    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one road mark record";
530
531
    throw CException(_HERE_,error.str());
  }
532
  else if(lane_info.roadMark().size()==1)
533
  {
534
535
536
537
538
539
540
541
    if(lane_info.roadMark().begin()->type1().present())
    {
      if(lane_info.roadMark().begin()->type1().get()=="none")
        mark=OD_MARK_NONE;
      else if(lane_info.roadMark().begin()->type1().get()=="solid")
        mark=OD_MARK_SOLID;
      else if(lane_info.roadMark().begin()->type1().get()=="broken")
        mark=OD_MARK_BROKEN;
542
      else if(lane_info.roadMark().begin()->type1().get()=="solid solid")
543
        mark=OD_MARK_SOLID_SOLID;
544
      else if(lane_info.roadMark().begin()->type1().get()=="solid broken")
545
        mark=OD_MARK_SOLID_BROKEN;
546
      else if(lane_info.roadMark().begin()->type1().get()=="broken solid")
547
        mark=OD_MARK_BROKEN_SOLID;
548
      else if(lane_info.roadMark().begin()->type1().get()=="broken broken")
549
550
551
552
        mark=OD_MARK_BROKEN_BROKEN;
      else
        mark=OD_MARK_NONE;
    }
553
554
555
    else
      mark=OD_MARK_NONE;
  }
Sergi Hernandez's avatar
Sergi Hernandez committed
556
  this->right_mark=mark;
557

558
  this->set_parent_segment(segment);
559
560
}

561
unsigned int COpendriveLane::get_num_nodes(void) const
562
563
564
565
{
  return this->nodes.size();
}

566
const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index) const
567
{
Sergi Hernandez's avatar
Sergi Hernandez committed
568
569
  std::stringstream error;

570
571
572
  if(index>=0 && index<this->nodes.size())
    return *this->nodes[index];
  else
Sergi Hernandez's avatar
Sergi Hernandez committed
573
574
575
576
  {
    error << "Invalid node index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
    throw CException(_HERE_,error.str());
  }
577
578
}

579
580
581
582
583
584
585
unsigned int COpendriveLane::get_num_next_lanes(void) const
{
  return this->next.size();
}

const COpendriveLane &COpendriveLane::get_next_lane(unsigned int index) const
{
Sergi Hernandez's avatar
Sergi Hernandez committed
586
587
  std::stringstream error;

588
589
590
  if(index>=0 && index<this->next.size())
    return *this->next[index];
  else
Sergi Hernandez's avatar
Sergi Hernandez committed
591
592
593
594
  {
    error << "Invalid next lane index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
    throw CException(_HERE_,error.str());
  }
595
596
597
598
599
600
601
602
603
}

unsigned int COpendriveLane::get_num_prev_lanes(void) const
{
  return this->prev.size();
}

const COpendriveLane &COpendriveLane::get_prev_lane(unsigned int index) const
{
Sergi Hernandez's avatar
Sergi Hernandez committed
604
605
  std::stringstream error;

606
607
608
  if(index>=0 && index<this->prev.size())
    return *this->prev[index];
  else
Sergi Hernandez's avatar
Sergi Hernandez committed
609
610
611
612
  {
    error << "Invalid previous lane index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
    throw CException(_HERE_,error.str());
  }
613
614
}

615
const COpendriveRoadSegment &COpendriveLane::get_segment(void) const
616
{
617
  return *this->segment;
618
619
}

620
double COpendriveLane::get_width(void) const
621
{
622
  return this->width/this->scale_factor;
623
624
}

625
double COpendriveLane::get_speed(void) const
626
627
628
629
{
  return this->speed;
}

630
631
632
633
634
double COpendriveLane::get_offset(void) const
{
  return this->offset/this->scale_factor;
}

635
636
637
638
639
640
641
642
double COpendriveLane::get_center_offset(void) const
{
  if(this->id<0)
    return (this->offset-this->width/2.0)/this->scale_factor;
  else
    return (this->offset+this->width/2.0)/this->scale_factor;
}

643
644
645
646
647
unsigned int COpendriveLane::get_index(void) const
{
  return this->index;
}

648
int COpendriveLane::get_id(void) const
649
{
650
  return this->id;
651
652
}

653
TOpendriveWorldPose COpendriveLane::get_start_pose(void) const
654
{
655
656
  TOpendriveTrackPose track_pose;
  TOpendriveWorldPose world_pose;
Sergi Hernandez's avatar
Sergi Hernandez committed
657
  std::stringstream error;
658

659
  track_pose.t=0.0;
660
661
  if(this->id<0)// right lane
  {
662
    track_pose.s=0.0;
663
    track_pose.heading=0.0;
664
665
666
  }
  else
  {
667
668
    track_pose.s=this->segment->get_length();
    track_pose.heading=3.14159;
669
  }
670
  world_pose=this->transform_to_world_pose(track_pose);
671

672
  return world_pose;
673
674
}

675
TOpendriveWorldPose COpendriveLane::get_end_pose(void) const
676
{
677
678
  TOpendriveTrackPose track_pose;
  TOpendriveWorldPose world_pose;
Sergi Hernandez's avatar
Sergi Hernandez committed
679
  std::stringstream error;
680
  
681
682
683
684
685
  track_pose.t=0.0;
  if(this->id<0)// right_lane
  {
    track_pose.s=this->segment->get_length();
    track_pose.heading=0.0;
686
687
  }
  else
688
689
690
  {
    track_pose.s=0.0;
    track_pose.heading=3.14159;
691
  }
692
  world_pose=this->transform_to_world_pose(track_pose);
693

694
  return world_pose;
695
696
}

697
double COpendriveLane::get_length(void) const
698
{
Sergi Hernandez's avatar
Sergi Hernandez committed
699
  std::stringstream error;
700
701
702
703
  double length=0.0;

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
704
    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
705
    {
706
707
      if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
        length+=this->nodes[i]->links[j]->get_length();
708
709
710
711
712
713
    }
  }

  return length;
}

Sergi Hernandez's avatar
Sergi Hernandez committed
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
TOpendriveWorldPose COpendriveLane::get_pose_at(double length)
{
  TOpendriveWorldPose world_pose={0};

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
    {
      if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
      {
        if((length-this->nodes[i]->links[j]->get_length())<0.0)
        {
          world_pose=this->nodes[i]->links[j]->get_pose_at(length);
          return world_pose;
        }
        else
          length-=this->nodes[i]->links[j]->get_length();
      }
    }
  }

  return world_pose;
}

double COpendriveLane::get_curvature_at(double length)
{
  double curvature;

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
    {
      if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
      {
        if((length-this->nodes[i]->links[j]->get_length())<0.0)
        {
          curvature=this->nodes[i]->links[j]->get_curvature_at(length);
          return curvature;
        }
        else
          length-=this->nodes[i]->links[j]->get_length();
      }
    }
  }

  return 0.0;
}

762
TOpendriveLocalPose COpendriveLane::transform_to_local_pose(TOpendriveTrackPose &track) const
763
{
764
  TOpendriveTrackPose pose;
765

766
767
768
  pose=track;
  pose.t+=this->get_center_offset();
  return this->segment->transform_to_local_pose(pose);
769
770
}

771
TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose &track) const
772
{ 
773
  TOpendriveTrackPose pose;
774

775
776
777
  pose=track;
  pose.t+=this->get_center_offset();
  return this->segment->transform_to_world_pose(pose);
778
779
}

Sergi Hernandez's avatar
Sergi Hernandez committed
780
unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
781
{
782
  double dist,min_dist=std::numeric_limits<double>::max(),length;
783
  TOpendriveWorldPose found_pose;
Sergi Hernandez's avatar
Sergi Hernandez committed
784
  unsigned int closest_index=-1;
785
786
787

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
788
789
    length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
    if(length<std::numeric_limits<double>::max())
790
    {
791
792
793
794
795
796
797
      dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
      if(dist<min_dist)
      {
        min_dist=dist;
        closest_index=i;
        distance=length;
      }
798
799
800
801
802
803
    }
  }

  return closest_index;
}

Sergi Hernandez's avatar
Sergi Hernandez committed
804
805
806
807
808
809
810
811
812
813
814
815
const COpendriveRoadNode &COpendriveLane::get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  unsigned int closest_index;

  closest_index=this->get_closest_node_index(pose,distance,angle_tol);
  if(closest_index==(unsigned int)-1)
    throw CException(_HERE_,"Impossible to find the closest node");
  return *this->nodes[closest_index];
}

double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
{
816
  double dist,min_dist=std::numeric_limits<double>::max(),distance,length;
Sergi Hernandez's avatar
Sergi Hernandez committed
817
818
819
820
821
822
823
824
825
  TOpendriveWorldPose found_pose;
  unsigned int closest_index=-1;
  COpendriveLink *link;

  closest_pose.x=std::numeric_limits<double>::max();
  closest_pose.y=std::numeric_limits<double>::max();
  closest_pose.heading=std::numeric_limits<double>::max();
  for(unsigned int i=0;i<this->nodes.size();i++)
  {
826
827
    length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
    if(length<std::numeric_limits<double>::max())
Sergi Hernandez's avatar
Sergi Hernandez committed
828
    {
829
830
831
832
833
834
835
836
      dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
      if(dist<min_dist)
      {
        min_dist=dist;
        closest_index=i;
        closest_pose=found_pose;
        distance=length;
      }
Sergi Hernandez's avatar
Sergi Hernandez committed
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
    }
  }

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    if(i<closest_index)
    {
      link=this->nodes[i]->get_link_with(this->nodes[i+1]);
      if(link!=NULL)
        distance+=link->get_length();
    }
    else
      break;
  }

  return distance;
}

855
std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
856
{
857
  out << "  Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
858
859
860
861
862
863
  out << "    width: " << lane.get_width() << std::endl;
  out << "    speed: " << lane.get_speed() << std::endl;
  if(lane.segment!=NULL)
    out << "    Parent road segment: " << lane.segment->get_name() << std::endl;
  else 
    out << "    No parent segment" << std::endl;
864
865
  if(lane.left_lane==NULL)
    out << "    No left lane (";
866
  else
867
    out << "    Left lane " << lane.left_lane->get_id() << " (";
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
  switch(lane.left_mark)
  {
    case OD_MARK_NONE:
      out << "no mark)" << std::endl;
      break;
    case OD_MARK_SOLID:
      out << "solid)" << std::endl;
      break;
    case OD_MARK_BROKEN:
      out << "broken)" << std::endl;
      break;
    case OD_MARK_SOLID_SOLID:
      out << "solid solid)" << std::endl;
      break;
    case OD_MARK_SOLID_BROKEN:
      out << "solid broken)" << std::endl;
      break;
    case OD_MARK_BROKEN_SOLID:
      out << "broken solid)" << std::endl;
      break;
    case OD_MARK_BROKEN_BROKEN:
      out << "broken broken)" << std::endl;
      break;
  }
892
893
  if(lane.right_lane==NULL)
    out << "    No right lane (";
894
  else
895
    out << "    Right lane " << lane.right_lane->get_id() << " (";
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
  switch(lane.right_mark)
  {
    case OD_MARK_NONE:
      out << "no mark)" << std::endl;
      break;
    case OD_MARK_SOLID:
      out << "solid)" << std::endl;
      break;
    case OD_MARK_BROKEN:
      out << "broken)" << std::endl;
      break;
    case OD_MARK_SOLID_SOLID:
      out << "solid solid)" << std::endl;
      break;
    case OD_MARK_SOLID_BROKEN:
      out << "solid broken)" << std::endl;
      break;
    case OD_MARK_BROKEN_SOLID:
      out << "broken solid)" << std::endl;
      break;
    case OD_MARK_BROKEN_BROKEN:
      out << "broken broken)" << std::endl;
      break;
  }
920
921
  out << "    Number of nodes: " << lane.nodes.size() << std::endl;
  for(unsigned int i=0;i<lane.nodes.size();i++)
922
    out << *lane.nodes[i];
923

924
  return out;
925
926
927
928
}

COpendriveLane::~COpendriveLane()
{
929
  this->nodes.clear();
930
931
  this->prev.clear();
  this->next.clear();
932
933
934
935
  this->left_lane=NULL;
  this->left_mark=OD_MARK_NONE;
  this->right_lane=NULL;
  this->right_mark=OD_MARK_NONE;
936
937
938
939
940
941
  this->segment=NULL;
  this->resolution=DEFAULT_RESOLUTION;
  this->scale_factor=DEFAULT_SCALE_FACTOR;
  this->width=0.0;
  this->speed=0.0;
  this->offset=0.0;
Sergi Hernandez's avatar
Sergi Hernandez committed
942
  this->id=0;
943
  this->index=-1;
944
}