opendrive_road_node.h 3.31 KB
Newer Older
1
2
3
4
#ifndef _OPENDRIVE_ROAD_NODE_H
#define _OPENDRIVE_ROAD_NODE_H

#include <vector>
5
#include "opendrive_common.h"
6
7
#include "opendrive_link.h"
#include "opendrive_lane.h"
8
#include "opendrive_road_segment.h"
9

Sergi Hernandez's avatar
Sergi Hernandez committed
10
11
12
13
typedef struct
{
  COpendriveLane * lane;
  COpendriveRoadSegment *segment;
14
15
  double start_curvature;
  double end_curvature;
Sergi Hernandez's avatar
Sergi Hernandez committed
16
17
}TOpendriveRoadNodeParent;

18
19
class COpendriveRoadNode
{
20
21
  friend class COpendriveLane;
  friend class COpendriveRoadSegment;
22
  friend class COpendriveRoad;
23
  friend class COpendriveLink;
24
25
26
27
  private:
    std::vector<COpendriveLink *> links;
    double resolution;
    double scale_factor;
28
    TOpendriveWorldPose pose;
Sergi Hernandez's avatar
Sergi Hernandez committed
29
    std::vector<TOpendriveRoadNodeParent> parents;
30
    unsigned int index;
31
  protected:
32
    COpendriveRoadNode();
33
    COpendriveRoadNode *clone(link_up_ref_t &new_link_ref);
Sergi Hernandez's avatar
Sergi Hernandez committed
34
35
    void free(void);
    bool add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
36
37
    void add_link(COpendriveLink *link);
    void remove_link(COpendriveLink *link);
Sergi Hernandez's avatar
Sergi Hernandez committed
38
39
    bool has_link(COpendriveLink *link);
    COpendriveLink *get_link_with(COpendriveRoadNode *node);
40
41
    void set_resolution(double res);
    void set_scale_factor(double scale);
42
    void set_index(unsigned int index);
43
    void set_pose(TOpendriveWorldPose &start);
44
    void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double start_curvature,double end_curvature);
Sergi Hernandez's avatar
Sergi Hernandez committed
45
46
    TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane);
    TOpendriveRoadNodeParent *get_parent_with_segment(const COpendriveRoadSegment *segment);
47
    bool updated(node_up_ref_t &refs);
Sergi Hernandez's avatar
Sergi Hernandez committed
48
    COpendriveRoadNode *get_original_node(node_up_ref_t &node_refs);
49
50
    void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
    void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
51
52
    void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance);
    void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance);
53
  public:
54
55
    double get_resolution(void) const;
    double get_scale_factor(void) const;
56
    unsigned int get_index(void) const;
Sergi Hernandez's avatar
Sergi Hernandez committed
57
    unsigned int get_num_parents(void) const;
58
    const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
Sergi Hernandez's avatar
Sergi Hernandez committed
59
    const COpendriveLane &get_parent_lane(unsigned int index) const;
60
    TOpendriveWorldPose get_pose(void) const;
61
62
    unsigned int get_num_links(void) const;
    const COpendriveLink &get_link(unsigned int index) const;
Sergi Hernandez's avatar
Sergi Hernandez committed
63
    const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
64
    bool has_link_with(const COpendriveRoadNode &node) const;
65
    unsigned int get_closest_link(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
66
67
    double get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes=false,double angle_tol=0.1) const;
    double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes=false,double angle_tol=0.1) const;
68
    void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes=false,double angle_tol=0.1) const;
69
70
71
72
73
    friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
    ~COpendriveRoadNode();
};

#endif