opendrive_geometry.cpp 4.14 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
#include "opendrive_geometry.h"
#include <cmath>

COpendriveGeometry::COpendriveGeometry()
{
  this->min_s = 0.0;
  this->max_s = 0.0;
  this->pose.x = 0.0;
  this->pose.y = 0.0;
  this->pose.heading = 0.0;
11
  this->scale_factor=DEFAULT_SCALE_FACTOR;
12
13
14
15
16
17
18
19
20
}

COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
{
  this->min_s = object.min_s;
  this->max_s = object.max_s;
  this->pose.x = object.pose.x;
  this->pose.y = object.pose.y;
  this->pose.heading = object.pose.heading;
21
  this->scale_factor = object.scale_factor;
22
23
24
25
}

void COpendriveGeometry::print(std::ostream& out)
{
26
  TOpendriveWorldPose tmp_pose=this->get_start_pose();
Sergi Hernandez's avatar
Sergi Hernandez committed
27

28
29
  out << "        Geometry " << this->get_name() << std::endl; 
  out << "          lenght: " << this->get_length() << std::endl;
30
  out << "          pose: x = " << tmp_pose.x << ", y = " << tmp_pose.y << ", heading = " << tmp_pose.heading << std::endl;
31
32
33
34
35
36
37
38
39
40
41
42
}

void COpendriveGeometry::load(const planView::geometry_type &geometry_info)
{
  this->min_s = (geometry_info.s().present() ? geometry_info.s().get() : 0.0);
  this->max_s = min_s + (geometry_info.length().present() ? geometry_info.length().get() : 0.0);
  this->pose.x = (geometry_info.x().present() ? geometry_info.x().get() : 0.0);
  this->pose.y = (geometry_info.y().present() ? geometry_info.y().get() : 0.0);
  this->pose.heading = (geometry_info.hdg().present() ? geometry_info.hdg().get() : 0.0);
  this->load_params(geometry_info);
}

43
44
45
46
47
void COpendriveGeometry::set_scale_factor(double scale)
{
  this->scale_factor=scale;
}

48
void COpendriveGeometry::set_start_pose(TOpendriveWorldPose &pose)
Sergi Hernandez's avatar
Sergi Hernandez committed
49
{
50
51
52
  this->pose.x=pose.x*this->scale_factor;
  this->pose.y=pose.y*this->scale_factor;
  this->pose.heading=pose.heading;
Sergi Hernandez's avatar
Sergi Hernandez committed
53
54
55
56
57
58
59
60
61
62
63
64
}

void COpendriveGeometry::set_max_s(double s)
{
  this->max_s=s*this->scale_factor;
}

void COpendriveGeometry::set_min_s(double s)
{
  this->min_s=s*this->scale_factor;
}

65
bool COpendriveGeometry::get_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
66
67
68
69
{
  return this->transform_local_pose(track,local);
}

70
bool COpendriveGeometry::get_world_pose(const TOpendriveTrackPose &track,TOpendriveWorldPose &world) const
71
{
72
  TOpendriveLocalPose local;
73
74
75
76

  if(this->transform_local_pose(track,local))
  {
    world.heading=normalize_angle(this->pose.heading+local.heading);
77
78
    world.x=local.u*std::cos(this->pose.heading)-local.v*std::sin(this->pose.heading)+this->pose.x/this->scale_factor;
    world.y=local.u*std::sin(this->pose.heading)+local.v*std::cos(this->pose.heading)+this->pose.y/this->scale_factor;
79
80
81
82
83
84
    return true;
  }
  else
    return false;
}

85
bool COpendriveGeometry::in_range(double s) const
86
{
87
  if((s<(this->max_s/this->scale_factor)) && (s>=(this->min_s/this->scale_factor)))
88
89
90
91
92
    return true;
  else 
    return false;
}

93
double COpendriveGeometry::get_length(void) const
94
{
95
  return (this->max_s-this->min_s)/this->scale_factor;
96
97
}

Sergi Hernandez's avatar
Sergi Hernandez committed
98
99
100
101
102
103
104
105
106
107
double COpendriveGeometry::get_max_s(void) const
{
  return this->max_s/this->scale_factor;
}

double COpendriveGeometry::get_min_s(void) const
{
  return this->min_s/this->scale_factor;
}

108
TOpendriveWorldPose COpendriveGeometry::get_start_pose(void) const
Sergi Hernandez's avatar
Sergi Hernandez committed
109
{
110
  TOpendriveWorldPose tmp_pose;
Sergi Hernandez's avatar
Sergi Hernandez committed
111
  
112
113
114
  tmp_pose.x=this->pose.x/this->scale_factor;
  tmp_pose.y=this->pose.y/this->scale_factor;
  tmp_pose.heading=this->pose.heading;
Sergi Hernandez's avatar
Sergi Hernandez committed
115

116
  return tmp_pose;
Sergi Hernandez's avatar
Sergi Hernandez committed
117
118
}

119
120
121
122
123
124
125
126
127
128
129
130
131
TOpendriveWorldPose COpendriveGeometry::get_end_pose(void) const
{
  TOpendriveTrackPose track_pose;
  TOpendriveWorldPose world_pose;
 
  track_pose.s=this->get_length();
  track_pose.t=0.0;
  track_pose.heading=0.0;
  this->get_world_pose(track_pose,world_pose);

  return world_pose;
}

132
133
134
135
136
137
138
void COpendriveGeometry::operator=(const COpendriveGeometry &object)
{
  this->min_s = object.min_s;
  this->max_s = object.max_s;
  this->pose.x = object.pose.x;
  this->pose.y = object.pose.y;
  this->pose.heading = object.pose.heading;
139
  this->scale_factor = object.scale_factor;
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
}

std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom)
{
  geom.print(out);
  return out;
}

COpendriveGeometry::~COpendriveGeometry()
{
  this->min_s = 0.0;
  this->max_s = 0.0;
  this->pose.x = 0.0;
  this->pose.y = 0.0;
  this->pose.heading = 0.0;
155
  this->scale_factor = DEFAULT_SCALE_FACTOR;
156
157
}