opendrive_road.cpp 30.7 KB
Newer Older
1
#include "opendrive_road.h"
2
#include "exceptions.h"
3
4
#include <sys/types.h>
#include <sys/stat.h>
5
#include <math.h>
6
7
8
9
10
11
12

COpendriveRoad::COpendriveRoad()
{
  this->resolution=DEFAULT_RESOLUTION;
  this->scale_factor=DEFAULT_SCALE_FACTOR;
  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
  this->segments.clear();
13
  this->nodes.clear();
14
  this->lanes.clear();
15
16
17
18
19
}

COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
{
  COpendriveRoadSegment *segment;
20
  COpendriveRoadNode *node;
21
22
23
24
  COpendriveLane *lane;
  segment_up_ref_t new_segment_ref;
  node_up_ref_t new_node_ref;
  lane_up_ref_t new_lane_ref;
25

26
  this->free();
27
28
29
  this->resolution=object.resolution;
  this->scale_factor=object.scale_factor;
  this->min_road_length=object.min_road_length;
30
31
32
33
34
35
  for(unsigned int i=0;i<object.lanes.size();i++)
  {
    lane=new COpendriveLane(*object.lanes[i]);
    this->lanes.push_back(lane);
    new_lane_ref[object.lanes[i]]=lane;
  }
36
37
38
39
  for(unsigned int i=0;i<object.nodes.size();i++)
  {
    node=new COpendriveRoadNode(*object.nodes[i]);
    this->nodes.push_back(node);
40
    new_node_ref[object.nodes[i]]=node;
41
  }
42
43
  for(unsigned int i=0;i<object.segments.size();i++)
  {
44
    segment=new COpendriveRoadSegment(*object.segments[i]);
45
46
47
48
    this->segments.push_back(segment);
    new_segment_ref[object.segments[i]]=segment;
  }
  // update references
Sergi Hernandez's avatar
Sergi Hernandez committed
49
  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
50
51
}

Sergi Hernandez's avatar
Sergi Hernandez committed
52
COpendriveRoadSegment *COpendriveRoad::get_segment_by_id(std::string &id)
53
{
Sergi Hernandez's avatar
Sergi Hernandez committed
54
55
  std::stringstream error;

56
57
  for(unsigned int i=0;i<this->segments.size();i++)
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
58
59
    if(this->segments[i]->get_id()==(unsigned int)std::stoi(id))
      return this->segments[i];
60
61
  }

Sergi Hernandez's avatar
Sergi Hernandez committed
62
63
  error << "No road segment with the " << id << " ID";
  throw CException(_HERE_,error.str());
64
65
}

66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
void COpendriveRoad::free(void)
{
  for(unsigned int i=0;i<this->segments.size();i++)
  {
    delete this->segments[i];
    this->segments[i]=NULL;
  }
  this->segments.clear();
  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    delete this->nodes[i];
    this->nodes[i]=NULL;
  }
  this->nodes.clear();
  for(unsigned int i=0;i<this->lanes.size();i++)
  {
    delete this->lanes[i];
    this->lanes[i]=NULL;
  }
  this->lanes.clear();
}

88
89
void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
{
90
  std::string predecessor_id,successor_id;
91
  std::string predecessor_contact,successor_contact;
Sergi Hernandez's avatar
Sergi Hernandez committed
92
93
  COpendriveRoadSegment *prev_road,*road,*next_road;
  std::stringstream error;
94

95
96
  for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it)
  {
97
    // get current segment
Sergi Hernandez's avatar
Sergi Hernandez committed
98
    road=this->get_segment_by_id(road_it->id().get());
99
100
    // get predecessor and successor
    if(road_it->lane_link().present())
101
    {
102
      if(road_it->lane_link().get().predecessor().present())// predecessor present
103
      {
104
        if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")// previous segment is a road
105
        {
106
          predecessor_id=road_it->lane_link().get().predecessor().get().elementId().get();
107
108
          predecessor_contact=road_it->lane_link().get().predecessor().get().contactPoint().get();
        }
109
110
111
112
      }
      if(road_it->lane_link().get().successor().present())// successor present
      {
        if(road_it->lane_link().get().successor().get().elementType().get()=="road")
113
        {
114
          successor_id=road_it->lane_link().get().successor().get().elementId().get();
115
116
          successor_contact=road_it->lane_link().get().successor().get().contactPoint().get();
        }
117
118
119
120
121
122
      }
    }
    if(std::stoi(road_it->junction().get())==-1)// non junction road segments
    {
      if(!predecessor_id.empty())
      {
Sergi Hernandez's avatar
Sergi Hernandez committed
123
124
        prev_road=this->get_segment_by_id(predecessor_id);
        prev_road->link_segment(road);
125
126
127
128
        predecessor_id.clear();
      }
      if(!successor_id.empty())
      {
Sergi Hernandez's avatar
Sergi Hernandez committed
129
130
        next_road=this->get_segment_by_id(successor_id);
        road->link_segment(next_road);
131
132
133
134
135
136
137
138
        successor_id.clear();
      }
    }
    else// junction segment
    {
      for(OpenDRIVE::junction_iterator junction_it(open_drive.junction().begin());junction_it!=open_drive.junction().end();++junction_it)
      {
        for(junction::connection_iterator connection_it(junction_it->connection().begin()); connection_it!=junction_it->connection().end();++connection_it)
139
        {
140
141
142
143
144
          std::string incoming_road_id;
          std::string connecting_road_id;
          if(connection_it->incomingRoad().present())
            incoming_road_id=connection_it->incomingRoad().get();
          else
Sergi Hernandez's avatar
Sergi Hernandez committed
145
146
147
148
          {
            error << "Connectivity information missing for segment " << road->get_name() << ": No incomming segment";
            throw CException(_HERE_,error.str());
          }
149
150
151
          if(connection_it->connectingRoad().present())
            connecting_road_id=connection_it->connectingRoad().get();
          else
Sergi Hernandez's avatar
Sergi Hernandez committed
152
153
154
155
          {
            error << "Connectivity information missing for segment " << road->get_name() << ": No connecting segment";
            throw CException(_HERE_,error.str());
          }
156
          if(predecessor_id.compare(incoming_road_id)==0 && successor_id.compare(connecting_road_id)==0)// this is the connection
157
          {
Sergi Hernandez's avatar
Sergi Hernandez committed
158
159
            prev_road=this->get_segment_by_id(predecessor_id);
            next_road=this->get_segment_by_id(successor_id);
160
161
162
163
164
165
166
            for(connection::laneLink_iterator lane_link_it(connection_it->laneLink().begin()); lane_link_it!=connection_it->laneLink().end();++lane_link_it)
            {
              int from_lane_id;
              int to_lane_id;
              if(lane_link_it->from().present())
                from_lane_id=lane_link_it->from().get();
              else
Sergi Hernandez's avatar
Sergi Hernandez committed
167
168
169
170
              {
                error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing from identifier";
                throw CException(_HERE_,error.str());
              }
171
172
173
              if(lane_link_it->to().present())
                to_lane_id=lane_link_it->to().get();
              else
Sergi Hernandez's avatar
Sergi Hernandez committed
174
175
176
177
              {
                error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing to identifier";
                throw CException(_HERE_,error.str());
              }
178
              if(predecessor_contact=="end")
Sergi Hernandez's avatar
Sergi Hernandez committed
179
                prev_road->link_segment(road,from_lane_id,false,-1,true);
180
              else 
Sergi Hernandez's avatar
Sergi Hernandez committed
181
                prev_road->link_segment(road,from_lane_id,true,-1,true);
182
183
              TOpendriveWorldPose end=road->get_lane(-1).get_end_pose();
              TOpendriveWorldPose start;
184
185
              if(successor_contact=="end")
              {
Sergi Hernandez's avatar
Sergi Hernandez committed
186
                if(to_lane_id<0)
187
                  start=next_road->get_lane(to_lane_id).get_end_pose();
Sergi Hernandez's avatar
Sergi Hernandez committed
188
                else
189
                  start=next_road->get_lane(to_lane_id).get_start_pose();
190
                if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
Sergi Hernandez's avatar
Sergi Hernandez committed
191
                  road->link_segment(next_road,-1,false,to_lane_id,false);
192
193
194
              }
              else
              {
Sergi Hernandez's avatar
Sergi Hernandez committed
195
                if(to_lane_id<0)
196
                  start=next_road->get_lane(to_lane_id).get_start_pose();
Sergi Hernandez's avatar
Sergi Hernandez committed
197
                else
198
                  start=next_road->get_lane(to_lane_id).get_end_pose();
199
                if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
Sergi Hernandez's avatar
Sergi Hernandez committed
200
                  road->link_segment(next_road,-1,false,to_lane_id,true);
201
              }
202
            }
203
204
205
206
207
208
209
          }
        }
      }
    }
  }
}

210
unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node)
211
{
Sergi Hernandez's avatar
Sergi Hernandez committed
212
213
  std::stringstream error;

214
215
216
  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    if(this->nodes[i]==node)
Sergi Hernandez's avatar
Sergi Hernandez committed
217
218
219
220
    {
      error << "Node " << node->get_index() << " already present";
      throw CException(_HERE_,error.str());
    }
221
222
  }
  this->nodes.push_back(node);
223
224

  return this->nodes.size()-1;
225
226
}

Sergi Hernandez's avatar
Sergi Hernandez committed
227
bool COpendriveRoad::remove_node(COpendriveRoadNode *node)
228
229
230
231
232
233
234
235
236
{
  std::vector<COpendriveRoadNode *>::iterator it;

  for(it=this->nodes.begin();it!=this->nodes.end();)
  {
    if((*it)->get_index()==node->get_index())
    {
      delete *it;
      it=this->nodes.erase(it);
Sergi Hernandez's avatar
Sergi Hernandez committed
237
      return true;
238
239
240
241
    }
    else
      it++;
  }
Sergi Hernandez's avatar
Sergi Hernandez committed
242
243

  return false;
244
245
246
247
}

unsigned int COpendriveRoad::add_lane(COpendriveLane *lane)
{
Sergi Hernandez's avatar
Sergi Hernandez committed
248
249
  std::stringstream error;

250
251
252
  for(unsigned int i=0;i<this->lanes.size();i++)
  {
    if(this->lanes[i]==lane)
Sergi Hernandez's avatar
Sergi Hernandez committed
253
254
255
256
    {
      error << "Lane " << lane->get_index() << " already present";
      throw CException(_HERE_,error.str());
    }
257
258
259
260
261
262
  }
  this->lanes.push_back(lane);

  return this->lanes.size()-1;
}

Sergi Hernandez's avatar
Sergi Hernandez committed
263
bool COpendriveRoad::remove_lane(COpendriveLane *lane)
264
265
266
267
268
269
270
271
272
{
  std::vector<COpendriveLane *>::iterator it;

  for(it=this->lanes.begin();it!=this->lanes.end();)
  {
    if((*it)->get_index()==lane->get_index())
    {
      delete *it;
      it=this->lanes.erase(it);
Sergi Hernandez's avatar
Sergi Hernandez committed
273
      return true;
274
275
276
277
    }
    else
      it++;
  }
Sergi Hernandez's avatar
Sergi Hernandez committed
278
279

  return false;
280
281
}

282
283
284
void COpendriveRoad::complete_open_lanes(void)
{
  std::vector<COpendriveLane *>::iterator lane_it;
285
  TOpendriveWorldPose end_pose;
286
287
288
289
290
291

  // remove all nodes and lanes not present in the path
  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
  {
    if((*lane_it)->next.empty())// lane is not connected
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
292
      try{
293
294
        end_pose=(*lane_it)->get_end_pose();
        if(!this->node_exists_at(end_pose))// there is no node at the end pose
Sergi Hernandez's avatar
Sergi Hernandez committed
295
        {
296
          (*lane_it)->segment->add_empty_node(end_pose,(*lane_it));
Sergi Hernandez's avatar
Sergi Hernandez committed
297
298
299
300
301
          (*lane_it)->segment->link_neightbour_lanes();
        }
        else
          lane_it++;
      }catch(CException &e){
302
        lane_it++;
Sergi Hernandez's avatar
Sergi Hernandez committed
303
      }
304
305
306
307
308
309
    }
    else
      lane_it++;
  }   
}

310
311
312
313
314
315
316
317
318
bool COpendriveRoad::has_segment(COpendriveRoadSegment *segment)
{
  for(unsigned int i=0;i<this->segments.size();i++)
    if(this->segments[i]==segment)
      return true;
  
  return false;
}

Sergi Hernandez's avatar
Sergi Hernandez committed
319
void COpendriveRoad::add_segment(COpendriveRoadSegment *segment)
320
321
322
323
324
325
326
327
328
329
330
331
332
333
{
  for(unsigned int i=0;i<this->segments.size();i++)
    if(this->segments[i]->get_id()==segment->get_id())// segment is already present
      return;
  // add the new segment
  this->segments.push_back(segment);
  // add the lanes
  for(unsigned int i=1;i<=segment->get_num_right_lanes();i++)
    this->lanes.push_back(segment->lanes[-i]);
  for(unsigned int i=1;i<=segment->get_num_left_lanes();i++)
    this->lanes.push_back(segment->lanes[i]);
  // add the nodes
  for(unsigned int i=0;i<segment->get_num_nodes();i++)
    this->nodes.push_back(segment->nodes[i]);
Sergi Hernandez's avatar
Sergi Hernandez committed
334
335
  // update the road reference
  segment->set_parent_road(this);
336
337
}

Sergi Hernandez's avatar
Sergi Hernandez committed
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
std::vector<unsigned int> COpendriveRoad::update_path(node_up_ref_t &node_refs,std::vector<unsigned int> &path)
{
  std::vector<unsigned int> new_path;
  node_up_ref_t::iterator node_it;

  for(unsigned int i=0;i<path.size();i++)
  {
    for(node_it=node_refs.begin();node_it!=node_refs.end();node_it++)
    {
      if(node_it->first->get_index()==path[i])
      {
        new_path.push_back(node_it->second->get_index());
        break;
      }
    }
  }

  return new_path;
}

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
void COpendriveRoad::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::vector<COpendriveLane *>::iterator lane_it;
  std::vector<COpendriveRoadNode *>::iterator node_it;

  for(seg_it=this->segments.begin();seg_it!=this->segments.end();seg_it++)
  {
    if(segment_refs.find(*seg_it)!=segment_refs.end())
    {
      (*seg_it)=segment_refs[*seg_it];
      (*seg_it)->update_references(segment_refs,lane_refs,node_refs);
    }
    else if((*seg_it)->updated(segment_refs))
      (*seg_it)->update_references(segment_refs,lane_refs,node_refs);
  }
  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
    if(lane_refs.find(*lane_it)!=lane_refs.end())
      (*lane_it)=lane_refs[*lane_it];
  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];
}

void COpendriveRoad::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::vector<COpendriveLane *>::iterator lane_it;
  std::vector<COpendriveRoadNode *>::iterator node_it;

  for(seg_it=this->segments.begin();seg_it!=this->segments.end();)
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
390
    if((*seg_it)->updated(segment_refs))
391
392
393
394
395
396
397
398
399
    {
      (*seg_it)->clean_references(segment_refs,lane_refs,node_refs);
      seg_it++;
    }
    else
      seg_it=this->segments.erase(seg_it);
  }
  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
400
    if(!(*lane_it)->updated(lane_refs))
401
402
403
404
405
406
      lane_it=this->lanes.erase(lane_it);
    else
      lane_it++;
  }
  for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
407
    if(!(*node_it)->updated(node_refs))
408
409
410
411
412
413
      node_it=this->nodes.erase(node_it);
    else
      node_it++;
  }
}

Sergi Hernandez's avatar
Sergi Hernandez committed
414
415
416
417
418
419
420
421
422
423
void COpendriveRoad::reindex(void)
{
  // reindex lanes
  for(unsigned int i=0;i<this->get_num_lanes();i++)
    this->lanes[i]->set_index(i);
  // reindex nodes
  for(unsigned int i=0;i<this->get_num_nodes();i++)
    this->nodes[i]->set_index(i);
}

424
425
426
427
428
429
430
431
432
433
434
435
void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
{
  COpendriveLane *neightbour_lane;
  std::vector<COpendriveLane *>::iterator lane_it;
  bool present;

  // remove all nodes and lanes not present in the path
  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
  {
    present=false;
    for(unsigned int i=0;i<path_nodes.size();i++)
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
436
      if((*lane_it)->has_node_with_index(path_nodes[i]))
437
438
439
440
441
442
443
444
445
446
447
448
      {
        present=true;
        break;
      }
    } 
    if(!present)
    {
      neightbour_lane=(*lane_it)->left_lane;
      while(neightbour_lane!=NULL)
      {
        for(unsigned int i=0;i<path_nodes.size();i++)
        {
Sergi Hernandez's avatar
Sergi Hernandez committed
449
          if(neightbour_lane->has_node_with_index(path_nodes[i]))
450
451
452
453
454
455
456
457
458
459
460
461
462
463
          {
            present=true;
            break;
          }
        }
        if(present)
          break;
        neightbour_lane=neightbour_lane->left_lane;
      }
      neightbour_lane=(*lane_it)->right_lane;
      while(neightbour_lane!=NULL)
      {
        for(unsigned int i=0;i<path_nodes.size();i++)
        {
Sergi Hernandez's avatar
Sergi Hernandez committed
464
          if(neightbour_lane->has_node_with_index(path_nodes[i]))
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
          {
            present=true;
            break;
          }
        }
        if(present)
          break;
        neightbour_lane=neightbour_lane->right_lane;
      }
      if(!present)
      {
        (*lane_it)->segment->remove_lane(*lane_it);
        for(unsigned int i=0;i<(*lane_it)->nodes.size();i++)
          this->remove_node((*lane_it)->nodes[i]);
        lane_it=this->lanes.erase(lane_it);
      }
      else
        lane_it++;
    }
    else
      lane_it++;
  }
  // remove links to non-consecutive nodes
  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
    {
492
493
      if(!this->has_segment(this->nodes[i]->links[j]->segment))
        this->nodes[i]->remove_link(this->nodes[i]->links[j]);
494
495
496
497
    }
  } 
}

498
bool COpendriveRoad::node_exists_at(TOpendriveWorldPose &pose)
499
{
500
  TOpendriveWorldPose node_pose;
501
502
503

  for(unsigned int i=0;i<nodes.size();i++)
  {
504
    node_pose=this->nodes[i]->get_pose();
505
506
507
508
509
510
511
    if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
      return true;
  }

  return false;
}

512
COpendriveRoadNode* COpendriveRoad::get_node_at(TOpendriveWorldPose &pose)
513
{
514
  TOpendriveWorldPose node_pose;
515
516
517

  for(unsigned int i=0;i<nodes.size();i++)
  {
518
    node_pose=this->nodes[i]->get_pose();
519
520
521
522
523
524
525
    if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
      return this->nodes[i];
  }

  return NULL;
}

526
527
528
529
530
531
532
void COpendriveRoad::load(const std::string &filename)
{
  struct stat buffer;

  if(stat(filename.c_str(),&buffer)==0)
  {
    // delete any previous data
533
    this->free();
534
535
536
537
538
    // try to open the specified file
    try{
      std::unique_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate));
      for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it)
      {
539
        try{
540
          COpendriveRoadSegment *segment=new COpendriveRoadSegment();
541
542
          segment->set_resolution(this->resolution);
          segment->set_scale_factor(this->scale_factor);
543
544
          segment->set_min_road_length(this->min_road_length);
          segment->set_parent_road(this);
545
546
547
548
549
          segment->load(*road_it);
          this->segments.push_back(segment);
        }catch(CException &e){
          std::cout << e.what() << std::endl;
        }
550
      }
551
552
      // link segments
      this->link_segments(*open_drive);
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
    }catch (const xml_schema::exception& e){
      std::ostringstream os;
      os << e;
      /* handle exceptions */
      throw CException(_HERE_,os.str());
    }
  }
  else
    throw CException(_HERE_,"The .xodr file does not exist");
}

void COpendriveRoad::set_resolution(double res)
{
  this->resolution=res;

  // change the resolution of all current nodes
  for(unsigned int i=0;i<this->segments.size();i++)
    this->segments[i]->set_resolution(res);
}

573
double COpendriveRoad::get_resolution(void) const
574
575
576
577
578
579
580
581
582
583
584
585
586
{
  return this->resolution;
}

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

  // change the resolution of all current nodes
  for(unsigned int i=0;i<this->segments.size();i++)
    this->segments[i]->set_scale_factor(scale);
}

587
double COpendriveRoad::get_scale_factor(void) const
588
589
590
591
592
593
594
{
  return this->scale_factor;
}

void COpendriveRoad::set_min_road_length(double length)
{
  this->min_road_length=length;
595
596
597

  for(unsigned int i=0;i<this->segments.size();i++)
    this->segments[i]->set_min_road_length(length);
598
599
}

600
double COpendriveRoad::get_min_road_length(void) const
601
602
603
604
{
  return this->min_road_length;
}

605
unsigned int COpendriveRoad::get_num_segments(void) const
606
607
608
609
{
  return this->segments.size();
}

610
const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) const
611
{
Sergi Hernandez's avatar
Sergi Hernandez committed
612
613
  std::stringstream error;

614
615
616
  if(index>=0 && index<this->segments.size())
    return *this->segments[index];
  else
Sergi Hernandez's avatar
Sergi Hernandez committed
617
618
619
620
  {
    error << "Invalid segment index " << index;
    throw CException(_HERE_,error.str());
  }
621
622
}

Sergi Hernandez's avatar
Sergi Hernandez committed
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
unsigned int COpendriveRoad::get_num_lanes(void) const
{
  return this->lanes.size();
}

const COpendriveLane &COpendriveRoad::get_lane(unsigned int index) const
{
  std::stringstream error;

  if(index>=0 && index<this->lanes.size())
    return *this->lanes[index];
  else
  {
    error << "Invalid lane index " << index;
    throw CException(_HERE_,error.str());
  }
}

641
642
643
644
645
646
647
unsigned int COpendriveRoad::get_num_nodes(void) const
{
  return this->nodes.size();
}

const COpendriveRoadNode& COpendriveRoad::get_node(unsigned int index) const
{
Sergi Hernandez's avatar
Sergi Hernandez committed
648
649
  std::stringstream error;

650
651
652
  if(index>=0 && index<this->nodes.size())
    return *this->nodes[index];
  else
Sergi Hernandez's avatar
Sergi Hernandez committed
653
654
655
656
  {
    error << "Invalid node index " << index;
    throw CException(_HERE_,error.str());
  }
657
658
}

659
660
const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
{
Sergi Hernandez's avatar
Sergi Hernandez committed
661
662
  std::stringstream error;

663
664
665
  if(index>=0 && index<this->segments.size())
    return *this->segments[index];
  else
Sergi Hernandez's avatar
Sergi Hernandez committed
666
667
668
669
  {
    error << "Invalid node index " << index;
    throw CException(_HERE_,error.str());
  }
670
671
}

Sergi Hernandez's avatar
Sergi Hernandez committed
672
unsigned int COpendriveRoad::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
Sergi Hernandez's avatar
Sergi Hernandez committed
673
{
674
  double dist,min_dist=std::numeric_limits<double>::max(),length;
675
  TOpendriveWorldPose closest_pose;
Sergi Hernandez's avatar
Sergi Hernandez committed
676
  unsigned int closest_index=-1;
Sergi Hernandez's avatar
Sergi Hernandez committed
677
678

  for(unsigned int i=0;i<this->nodes.size();i++)
Sergi Hernandez's avatar
Sergi Hernandez committed
679
  {
680
681
    length=this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
    if(length<std::numeric_limits<double>::max())
Sergi Hernandez's avatar
Sergi Hernandez committed
682
    {
683
684
685
686
687
688
689
      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
      if(dist<min_dist)
      {
        min_dist=dist;
        closest_index=i;
        distance=length;
      }
Sergi Hernandez's avatar
Sergi Hernandez committed
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
    }
  }

  return closest_index;
}

const COpendriveRoadNode &COpendriveRoad::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];
}

unsigned int COpendriveRoad::get_closest_lane_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
708
  double dist,min_dist=std::numeric_limits<double>::max(),length;
Sergi Hernandez's avatar
Sergi Hernandez committed
709
710
711
712
  TOpendriveWorldPose closest_pose;
  unsigned int closest_index=-1;

  for(unsigned int i=0;i<this->lanes.size();i++)
Sergi Hernandez's avatar
Sergi Hernandez committed
713
  { 
714
715
716
717
718
719
720
721
722
723
    length=this->lanes[i]->get_closest_pose(pose,closest_pose,angle_tol);
    if(length<std::numeric_limits<double>::max())
    {
      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
      if(dist<min_dist)
      { 
        min_dist=dist;
        closest_index=i;
        distance=length;
      }
Sergi Hernandez's avatar
Sergi Hernandez committed
724
725
726
727
728
729
    }
  }

  return closest_index;
}

Sergi Hernandez's avatar
Sergi Hernandez committed
730
731
732
733
734
735
736
737
738
739
740
741
const COpendriveLane &COpendriveRoad::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  unsigned int closest_index;

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

unsigned int COpendriveRoad::get_closest_segment_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
742
  double dist,min_dist=std::numeric_limits<double>::max(),length;
Sergi Hernandez's avatar
Sergi Hernandez committed
743
744
745
746
747
  TOpendriveWorldPose closest_pose;
  unsigned int closest_index=-1;

  for(unsigned int i=0;i<this->segments.size();i++)
  { 
748
749
750
751
752
753
754
755
756
757
    length=this->segments[i]->get_closest_pose(pose,closest_pose,angle_tol);
    if(length<std::numeric_limits<double>::max())
    {
      dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
      if(dist<min_dist)
      { 
        min_dist=dist;
        closest_index=i;
        distance=length;
      }
Sergi Hernandez's avatar
Sergi Hernandez committed
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
    }
  }

  return closest_index;
}

const COpendriveRoadSegment &COpendriveRoad::get_closest_segment(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  unsigned int closest_index;

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

double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
775
776
{
  double dist,min_dist=std::numeric_limits<double>::max();
777
  TOpendriveWorldPose pose_found;
778
779
780
781
  double length,closest_length;

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
782
    length=this->nodes[i]->get_closest_pose(pose,pose_found,false,angle_tol);
783
    if(length<std::numeric_limits<double>::max())
784
    {
785
786
787
788
789
790
791
      dist=sqrt(pow(pose_found.x-pose.x,2.0)+pow(pose_found.y-pose.y,2.0));
      if(dist<min_dist)
      {
        min_dist=dist;
        closest_pose=pose_found;
        closest_length=length;
      }
792
793
794
795
796
797
    }
  }

  return closest_length;
}

Sergi Hernandez's avatar
Sergi Hernandez committed
798
void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
Sergi Hernandez's avatar
Sergi Hernandez committed
799
{
800
  std::vector<TOpendriveWorldPose> poses;
Sergi Hernandez's avatar
Sergi Hernandez committed
801
  std::vector<double> lengths;
802
  bool already_added;
Sergi Hernandez's avatar
Sergi Hernandez committed
803

804
  closest_poses.clear();
Sergi Hernandez's avatar
Sergi Hernandez committed
805
806
807
  closest_lengths.clear();
  for(unsigned int i=0;i<this->nodes.size();i++)
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
808
    this->nodes[i]->get_closest_poses(pose,poses,lengths,dist_tol,false,angle_tol);
809
    for(unsigned int j=0;j<poses.size();j++)
Sergi Hernandez's avatar
Sergi Hernandez committed
810
    {
811
812
813
814
815
816
817
818
819
820
821
822
823
      already_added=false;
      for(unsigned int k=0;k<closest_poses.size();k++)
        if(fabs(closest_poses[k].x-poses[j].x)<this->resolution &&
           fabs(closest_poses[k].y-poses[j].y)<this->resolution)
        {
          already_added=true;
          break;
        }
      if(!already_added)
      {
        closest_poses.push_back(poses[j]);
        closest_lengths.push_back(lengths[j]);
      }
Sergi Hernandez's avatar
Sergi Hernandez committed
824
825
826
827
    }
  }
}

828
std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road)
829
830
831
832
{
  segment_up_ref_t new_segment_ref;
  lane_up_ref_t new_lane_ref;
  node_up_ref_t new_node_ref;
Sergi Hernandez's avatar
Sergi Hernandez committed
833
  link_up_ref_t new_link_ref;
834
  COpendriveRoadNode *node,*next_node;
Sergi Hernandez's avatar
Sergi Hernandez committed
835
836
  COpendriveRoadSegment *segment,*new_segment;
//  CopendriveRoadSegment *original_seg1,*original_seg2;
837
  COpendriveLink *link;
838
839
  std::vector<unsigned int> new_path_nodes;
  unsigned int link_index;
840
841

  new_path_nodes.resize(path_nodes.size());
842
843
844
845
846
847

  new_road.free();
  new_road.set_resolution(this->resolution);
  new_road.set_scale_factor(this->scale_factor);
  new_road.set_min_road_length(this->min_road_length);

848
  for(unsigned int i=0;i<path_nodes.size()-1;i++)
849
  {
850
851
852
853
854
855
856
857
    node=this->nodes[path_nodes[i]];
    next_node=this->nodes[path_nodes[i+1]];
    link=node->get_link_with(next_node);
    if(link==NULL)
      throw CException(_HERE_,"The provided path has unconnected nodes");
    segment=link->segment;
    if(new_segment_ref.find(segment)==new_segment_ref.end())
    {
Sergi Hernandez's avatar
Sergi Hernandez committed
858
859
      new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
      new_road.add_segment(new_segment);
860
861
862
863
864
      new_segment_ref[segment]=new_segment;
    }
  }
  // add the last segment
  node=this->nodes[path_nodes[path_nodes.size()-1]];
Sergi Hernandez's avatar
Sergi Hernandez committed
865
  link_index=node->get_closest_link(end_pose);
866
  if(link_index==(unsigned int)-1)
867
    throw CException(_HERE_,"The provided path has unconnected nodes");
868
  link=node->links[link_index];
869
870
871
  segment=link->segment;
  if(new_segment_ref.find(segment)==new_segment_ref.end())
  {
Sergi Hernandez's avatar
Sergi Hernandez committed
872
873
    new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
    new_road.add_segment(new_segment);
874
875
    new_segment_ref[segment]=new_segment;
  }
876
877
878
879

  // add additional nodes not explicitly in the path
/*
  for(unsigned int i=0;i<this->segments.size();i++)
880
  {
881
    for(unsigned int j=0;j<new_road.segments.size();j++)
882
    {
883
884
      original_seg1=new_road.segments[j]->get_original_segment(new_segment_ref);
      for(unsigned int k=j+1;k<new_road.segments.size();k++)
885
      {
886
887
        original_seg2=new_road.segments[k]->get_original_segment(new_segment_ref);
        if(this->segments[i]->connects_segments(original_seg1,original_seg2)
888
        {
889
          if(!new_road.has_segment(new_segment_ref[this->segments[i]]))
890
          {
Sergi Hernandez's avatar
Sergi Hernandez committed
891
892
            new_segment=this->segment[k]->clone(new_node_ref,new_lane_ref,new_link_ref);
            new_road.add_segment(new_segment);
893
            new_segment_ref[segment]=new_segment;
894
          }
895
896
897
898
        }
      }
    }
  }
899
*/
900
  new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
901
  new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
902
  // remove unconnected elements
Sergi Hernandez's avatar
Sergi Hernandez committed
903
904
  new_road.prune(path_nodes);
  new_road.reindex();
905
906
  new_road.complete_open_lanes();

Sergi Hernandez's avatar
Sergi Hernandez committed
907
  return new_road.update_path(new_node_ref,path_nodes);
908
909
}

Sergi Hernandez's avatar
Sergi Hernandez committed
910
void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length,COpendriveRoad &new_road)
911
{
912
913
914
  segment_up_ref_t new_segment_ref;
  lane_up_ref_t new_lane_ref;
  node_up_ref_t new_node_ref;
Sergi Hernandez's avatar
Sergi Hernandez committed
915
916
917
918
  link_up_ref_t new_link_ref;
  unsigned int segment_index,node_index;
  TOpendriveWorldPose end_pose;
  COpendriveRoadSegment *segment,*new_segment,*next_segment;
919
  COpendriveRoadNode *node;
Sergi Hernandez's avatar
Sergi Hernandez committed
920
921
  double rem_length=length,distance,start_length;
  int node_side;
922

Sergi Hernandez's avatar
Sergi Hernandez committed
923
924
925
926
927
  new_road.free();
  new_road.set_resolution(this->resolution);
  new_road.set_scale_factor(this->scale_factor);
  new_road.set_min_road_length(this->min_road_length);
  node_index=this->get_closest_node_index(start_pose,distance);
928
  if(node_index==(unsigned int)-1)
Sergi Hernandez's avatar
Sergi Hernandez committed
929
    throw CException(_HERE_,"Start position does not belang to the road");
930
  node=this->nodes[node_index];
Sergi Hernandez's avatar
Sergi Hernandez committed
931
932
933
934
  segment_index=this->get_closest_segment_index(start_pose,distance);
  if(segment_index==(unsigned int)-1)
    throw CException(_HERE_,"Start position does not belang to the road");
  segment=this->segments[segment_index];
935
  node_side=segment->get_node_side(node);
Sergi Hernandez's avatar
Sergi Hernandez committed
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
  new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,&start_pose,NULL);
  start_length=new_segment->get_length()-distance;
  if(rem_length<start_length)
  {
    if(node_side<0)
      end_pose=new_segment->get_pose_at(rem_length);
    else
      end_pose=new_segment->get_pose_at(new_segment->get_length()-rem_length);
    segment=new_segment;
    new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
    delete segment;
  }
  rem_length-=new_segment->get_length();
  new_road.add_segment(new_segment);
  new_segment_ref[segment]=new_segment;
  while(rem_length>0)
  {
    next_segment=segment->get_next_segment(node_side);
    if(next_segment==NULL)
      break;
    if((rem_length-next_segment->get_length())<0.0)
    {
      if(node_side<0)
        end_pose=next_segment->get_pose_at(rem_length);
      else
        end_pose=next_segment->get_pose_at(next_segment->get_length()-rem_length);
      new_segment=next_segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
    }
    else
      new_segment=next_segment->clone(new_node_ref,new_lane_ref,new_link_ref);
    rem_length-=new_segment->get_length();
    new_road.add_segment(new_segment);
    new_segment_ref[next_segment]=new_segment;
    segment=next_segment;
  }
  length-=rem_length;
  new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
  new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref);
  new_road.reindex();
  new_road.complete_open_lanes();
976
977
}

978
979
void COpendriveRoad::operator=(const COpendriveRoad& object)
{
980
981
  COpendriveRoadSegment *segment;
  COpendriveRoadNode *node;
982
983
984
985
  COpendriveLane *lane;
  segment_up_ref_t new_segment_ref;
  node_up_ref_t new_node_ref;
  lane_up_ref_t new_lane_ref;
986

987
  this->free();
988
989
990
  this->resolution=object.resolution;
  this->scale_factor=object.scale_factor;
  this->min_road_length=object.min_road_length;
991
  for(unsigned int i=0;i<object.lanes.size();i++)
992
  {
993
994
995
    lane=new COpendriveLane(*object.lanes[i]);
    this->lanes.push_back(lane);
    new_lane_ref[object.lanes[i]]=lane;
996
  }
997

998
999
1000
  for(unsigned int i=0;i<object.nodes.size();i++)
  {
    node=new COpendriveRoadNode(*object.nodes[i]);
For faster browsing, not all history is shown. View entire blame