opendrive_lane.cpp 13.7 KB
Newer Older
1
#include "opendrive_lane.h"
2
#include "exceptions.h"
3
4
5
6

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

20
COpendriveLane::COpendriveLane(const COpendriveLane &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoadSegment *segment_ref)
21
{
22
  this->nodes.resize(object.nodes.size());
23
  for(unsigned int i=0;i<object.nodes.size();i++)
24
    this->nodes[i]=node_refs[object.nodes[i]];
25
26
27
28
  this->left_lane=object.left_lane;
  this->left_mark=object.left_mark;
  this->right_lane=object.right_lane;
  this->right_mark=object.right_mark;
29
  this->segment=segment_ref;
30
31
  this->resolution=object.resolution;
  this->scale_factor=object.scale_factor;
32
33
34
  this->width=object.width;
  this->speed=object.speed;
  this->offset=object.offset;
35
  this->id=object.id;
36
37
}

38
void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
39
40
41
42
{
  if(lane!=NULL)
  {
    if(this->get_num_nodes()!=lane->get_num_nodes())
43
44
      throw CException(_HERE_,"Impossible to liink lanes because they have different number of nodes");
    if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
45
    {
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
      if(this->id*lane->get_id()<0) // oposite directions
      {
        for(unsigned int i=0;i<this->get_num_nodes();i++)
        {
          this->nodes[i]->add_link(lane->nodes[lane->get_num_nodes()-i-1],this->left_mark);
          lane->nodes[lane->get_num_nodes()-i-1]->add_link(this->nodes[i],this->left_mark);
          lane->right_mark=this->left_mark;
        }
      }
      else
      {
        for(unsigned int i=0;i<this->get_num_nodes()-1;i++)
        {
          this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark);
          lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark);
          if(this->id>0)// left lanes
            this->right_mark=lane->left_mark;
          else// right lanes
            this->left_mark=lane->right_mark;
        }
      } 
      this->left_lane=lane;
      lane->right_lane=this;
    }
    else
      throw CException(_HERE_,"One of the lanes to link has no nodes");
72
73
74
  }
}

75
void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start, bool to_start)
76
{
77
78
  COpendriveRoadNode *start,*end;

79
80
  if(lane!=NULL)
  {
81
    if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
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
118
119
120
121
122
123
124
125
126
    {
      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];
        }
      }
      start->add_link(end,mark);
    }
127
128
    else 
      throw CException(_HERE_,"One of the lanes to link has no nodes");
129
130
131
  }
}

132
133
void COpendriveLane::add_node(COpendriveRoadNode *node)
{
134
135
136
  for(unsigned int i=0;i<this->nodes.size();i++)
    if(this->nodes[i]==node)
      return;
Sergi Hernandez's avatar
Sergi Hernandez committed
137
138
139
  // link the new node with the previous one in the current lane, if any
  if(this->nodes.size()>0)
    this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE);
140
141
  // add the new node
  this->nodes.push_back(node);
142
143
144
}

void COpendriveLane::set_resolution(double res)
145
146
147
{
  this->resolution=res;

148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
  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);
}

void COpendriveLane::set_offset(double offset)
{
  this->offset=offset;
}

165
void COpendriveLane::update_references(std::map<COpendriveLane *,COpendriveLane *> &lane_refs)
166
{
167
168
169
170
171
  this->left_lane=lane_refs[this->left_lane];
  this->right_lane=lane_refs[this->right_lane];

  for(unsigned int i=0;i<this->nodes.size();i++)
    this->nodes[i]->update_references(lane_refs);
172
173
174
175
176
}

void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegment *segment)
{
  std::stringstream error;
177
  opendrive_mark_t mark;
178
179
180
181
182
183
184

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    delete this->nodes[i];
    this->nodes[i]=NULL;
  }
  this->nodes.clear();
185
186
187
188
  this->left_lane=NULL;
  this->left_mark=OD_MARK_NONE;
  this->right_lane=NULL;
  this->right_mark=OD_MARK_NONE;
189
  this->id=lane_info.id().get();
190
  // import lane width
191
  if(lane_info.width().size()!=1)
192
  {
193
    error << "Only one width record supported for lane " << this->id;
194
195
    throw CException(_HERE_,error.str());
  }
196
  this->width=lane_info.width().begin()->a().get();
197
  // import lane speed
198
  if(lane_info.speed().size()!=1)
199
  {
200
    error << "Only one speed record supported for lane " << this->id;
201
202
    throw CException(_HERE_,error.str());
  }
203
204
  this->speed=lane_info.speed().begin()->max().get();
  //lane mark
205
206
207
  if(lane_info.roadMark().size()==0)
    mark=OD_MARK_NONE;
  else if(lane_info.roadMark().size()>1)
208
  {
209
    error << "Only one road mark supported for lane " << this->id;
210
211
    throw CException(_HERE_,error.str());
  }
212
  else if(lane_info.roadMark().size()==1)
213
  {
214
215
216
217
218
219
220
221
    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;
222
      else if(lane_info.roadMark().begin()->type1().get()=="solid solid")
223
        mark=OD_MARK_SOLID_SOLID;
224
      else if(lane_info.roadMark().begin()->type1().get()=="solid broken")
225
        mark=OD_MARK_SOLID_BROKEN;
226
      else if(lane_info.roadMark().begin()->type1().get()=="broken solid")
227
        mark=OD_MARK_BROKEN_SOLID;
228
      else if(lane_info.roadMark().begin()->type1().get()=="broken broken")
229
230
231
232
        mark=OD_MARK_BROKEN_BROKEN;
      else
        mark=OD_MARK_NONE;
    }
233
234
235
236
237
238
239
    else
      mark=OD_MARK_NONE;
  }
  if(this->id<0)//right lanes
    this->right_mark=mark;
  else
    this->left_mark=mark;
240
241
242
243

  this->segment=segment;
}

244
unsigned int COpendriveLane::get_num_nodes(void) const
245
246
247
248
{
  return this->nodes.size();
}

249
const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index) const
250
{
251
252
253
254
  if(index>=0 && index<this->nodes.size())
    return *this->nodes[index];
  else
    throw CException(_HERE_,"Invalid node index");
255
256
}

257
const COpendriveRoadSegment &COpendriveLane::get_segment(void) const
258
{
259
  return *this->segment;
260
261
}

262
double COpendriveLane::get_width(void) const
263
{
264
  return this->width/this->scale_factor;
265
266
}

267
double COpendriveLane::get_speed(void) const
268
269
270
271
{
  return this->speed;
}

272
273
274
275
276
277
double COpendriveLane::get_offset(void) const
{
  return this->offset/this->scale_factor;
}

int COpendriveLane::get_id(void) const
278
{
279
  return this->id;
280
281
}

282
TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
283
284
285
286
287
288
289
290
291
292
{
  TOpendriveTrackPoint track_point;
  TOpendriveWorldPoint world_point;

  track_point.heading=0.0;
  if(this->id<0)// right lane
  {
    track_point.t=this->get_offset()-this->get_width()/2.0;
    track_point.s=0.0;
    for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
Sergi Hernandez's avatar
Sergi Hernandez committed
293
      if(&this->nodes[0]->get_lane(i)==this)
294
295
296
297
298
        this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
  }
  else
  {
    track_point.t=this->get_offset()+this->get_width()/2.0;
299
    track_point.s=0.0;
300
    for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
Sergi Hernandez's avatar
Sergi Hernandez committed
301
      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
302
303
304
305
306
307
308
309
        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);

  return world_point;
}

310
TOpendriveWorldPoint COpendriveLane::get_end_point(void) const
311
312
313
314
315
316
317
{
  TOpendriveTrackPoint track_point;
  TOpendriveWorldPoint world_point;
  
  track_point.heading=0.0;
  if(this->id>0)// left lane
  { 
Sergi Hernandez's avatar
Sergi Hernandez committed
318
    track_point.t=this->get_offset()+this->get_width()/2.0;
319
320
321
322
323
324
    for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
      if(&this->nodes[0]->get_lane(i)==this)
      {
        track_point.s=this->nodes[0]->get_geometry(i).get_length();;
        this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
      }
325
326
327
  }
  else
  { 
Sergi Hernandez's avatar
Sergi Hernandez committed
328
    track_point.t=this->get_offset()-this->get_width()/2.0;
329
    for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
Sergi Hernandez's avatar
Sergi Hernandez committed
330
      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
331
332
333
334
335
336
337
338
339
340
341
      {
        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);

  return world_point;
}

342
double COpendriveLane::get_length(void) const
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
{
  double length=0.0;

  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)
        length+=this->nodes[i]->get_geometry(j).get_length();
    }
  }

  return length;
}

358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
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");
}

412
std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
413
{
414
  out << "  Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
415
416
417
418
419
420
  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;
421
422
  if(lane.left_lane==NULL)
    out << "    No left lane (";
423
  else
424
    out << "    Left lane " << lane.left_lane->get_id() << " (";
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
  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;
  }
449
450
  if(lane.right_lane==NULL)
    out << "    No right lane (";
451
  else
452
    out << "    Right lane " << lane.right_lane->get_id() << " (";
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
  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;
  }
477
478
  out << "    Number of nodes: " << lane.nodes.size() << std::endl;
  for(unsigned int i=0;i<lane.nodes.size();i++)
479
    out << *lane.nodes[i];
480

481
  return out;
482
483
484
485
}

COpendriveLane::~COpendriveLane()
{
486
  this->nodes.clear();
487
488
489
490
  this->left_lane=NULL;
  this->left_mark=OD_MARK_NONE;
  this->right_lane=NULL;
  this->right_mark=OD_MARK_NONE;
491
492
493
494
495
496
  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;
497
}