Skip to content
GitLab
Commit d37ad5b8 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Changed the Point name to Pose.

parent d2ed15f3
Hide whitespace changes
Inline Side-by-side
include/opendrive_arc.h
View file @ d37ad5b8
......@@ -11,7 +11,7 @@ class COpendriveArc : public COpendriveGeometry
protected:
COpendriveArc();
COpendriveArc(const COpendriveArc &object);
virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual void print(std::ostream &out);
virtual void load_params(const planView::geometry_type &geometry_info);
virtual std::string get_name(void);
......
include/opendrive_common.h
View file @ d37ad5b8
......@@ -24,21 +24,21 @@ typedef struct
double s;
double t;
double heading;
}TOpendriveTrackPoint;
}TOpendriveTrackPose;
typedef struct
{
double u;
double v;
double heading;
}TOpendriveLocalPoint;
}TOpendriveLocalPose;
typedef struct
{
double x;
double y;
double heading;
}TOpendriveWorldPoint;
}TOpendriveWorldPose;
typedef enum{OD_MARK_NONE,
OD_MARK_SOLID,
......
include/opendrive_geometry.h
View file @ d37ad5b8
......@@ -19,25 +19,25 @@ class COpendriveGeometry
double scale_factor;
double min_s; ///< Starting track coordenate "s" for the geometry.
double max_s; ///< Ending track coordenate "s" for the geometry.
TOpendriveWorldPoint pose;
virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const = 0;
TOpendriveWorldPose pose;
virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const = 0;
virtual void print(std::ostream &out);
virtual void load_params(const planView::geometry_type &geometry_info) = 0;
virtual std::string get_name(void)=0;
void set_scale_factor(double scale);
void set_start_point(TOpendriveWorldPoint &point);
void set_start_pose(TOpendriveWorldPose &pose);
void set_max_s(double s);
void set_min_s(double s);
public:
virtual COpendriveGeometry *clone(void) = 0;
bool get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
bool get_world_pose(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world) const;
bool get_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
bool get_world_pose(TOpendriveTrackPose &track,TOpendriveWorldPose &world) const;
bool in_range(double s) const;
double get_length(void) const;
virtual void get_curvature(double &start,double &end)=0;
double get_max_s(void) const;
double get_min_s(void) const;
TOpendriveWorldPoint get_start_point(void) const;
TOpendriveWorldPose get_start_pose(void) const;
void operator=(const COpendriveGeometry &object);
friend std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom);
virtual ~COpendriveGeometry();
......
include/opendrive_lane.h
View file @ d37ad5b8
......@@ -56,9 +56,9 @@ class COpendriveLane
bool updated(lane_up_ref_t &refs);
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);
void update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start=NULL);
void update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end=NULL);
COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);
void update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *start=NULL);
void update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *end=NULL);
COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
COpendriveLane *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
public:
unsigned int get_num_nodes(void) const;
......@@ -73,12 +73,12 @@ class COpendriveLane
double get_offset(void) const;
unsigned int get_index(void) const;
int get_id(void) const;
TOpendriveWorldPoint get_start_point(void) const;
TOpendriveWorldPoint get_end_point(void) const;
TOpendriveWorldPose get_start_pose(void) const;
TOpendriveWorldPose get_end_pose(void) const;
double get_length(void) const;
TOpendriveLocalPoint transform_to_local_point(TOpendriveTrackPoint &track);
TOpendriveWorldPoint transform_to_world_point(TOpendriveTrackPoint &track);
unsigned int get_closest_node_index(TOpendriveWorldPoint &point,double angle_tol=0.1);
TOpendriveLocalPose transform_to_local_pose(TOpendriveTrackPose &track);
TOpendriveWorldPose transform_to_world_pose(TOpendriveTrackPose &track);
unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double angle_tol=0.1);
friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
~COpendriveLane();
};
......
include/opendrive_line.h
View file @ d37ad5b8
......@@ -10,7 +10,7 @@ class COpendriveLine : public COpendriveGeometry
protected:
COpendriveLine();
COpendriveLine(const COpendriveGeometry &object);
virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual void print(std::ostream &out);
virtual void load_params(const planView::geometry_type &geometry_info);
virtual std::string get_name(void);
......
include/opendrive_link.h
View file @ d37ad5b8
......@@ -36,8 +36,8 @@ class COpendriveLink
void generate(double start_curvature,double end_curvature,double length,bool lane);
void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lanei_refs,node_up_ref_t &node_refs);
bool clean_references(node_up_ref_t &refs);
void update_start_pose(TOpendriveWorldPoint *start=NULL);
void update_end_pose(TOpendriveWorldPoint *end=NULL);
void update_start_pose(TOpendriveWorldPose *start=NULL);
void update_end_pose(TOpendriveWorldPose *end=NULL);
public:
const COpendriveRoadNode &get_prev(void) const;
const COpendriveRoadNode &get_next(void) const;
......@@ -46,7 +46,7 @@ class COpendriveLink
const COpendriveLane &get_parent_lane(void) const;
double get_resolution(void) const;
double get_scale_factor(void) const;
double find_closest_world_point(TOpendriveWorldPoint &world,TOpendriveWorldPoint &point);
double find_closest_world_pose(TOpendriveWorldPose &world,TOpendriveWorldPose &pose);
void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;
void get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length=0.0, double end_length=-1.0) const;
double get_length(void) const;
......
include/opendrive_object.h
View file @ d37ad5b8
......@@ -26,7 +26,7 @@ typedef struct
typedef struct
{
TOpendriveTrackPoint vertices[OD_MAX_VERTICES];
TOpendriveTrackPose vertices[OD_MAX_VERTICES];
double height[OD_MAX_VERTICES];
unsigned int num_vertices;
}TOpendrivePolygon;
......@@ -46,7 +46,7 @@ class COpendriveObject
private:
COpendriveRoadSegment *segment;
int id; ///< Object's id.
TOpendriveTrackPoint pose;
TOpendriveTrackPose pose;
TOpendriveObject object;
std::string type; ///< Object's OpenDrive type.
std::string name; ///< Object's name.
......@@ -60,8 +60,8 @@ class COpendriveObject
COpendriveObject();
COpendriveObject(const COpendriveObject& object);
double get_scale_factor(void);
TOpendriveTrackPoint get_track_pose(void) const;
TOpendriveWorldPoint get_world_pose(void) const;
TOpendriveTrackPose get_track_pose(void) const;
TOpendriveWorldPose get_world_pose(void) const;
int get_id(void) const;
std::string get_type(void) const;
std::string get_name(void) const;
......
include/opendrive_param_poly3.h
View file @ d37ad5b8
......@@ -21,7 +21,7 @@ class COpendriveParamPoly3 : public COpendriveGeometry
protected:
COpendriveParamPoly3();
COpendriveParamPoly3(const COpendriveParamPoly3 &object);
virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual void print(std::ostream &out);
virtual void load_params(const planView::geometry_type &geometry_info);
virtual std::string get_name(void);
......
include/opendrive_road.h
View file @ d37ad5b8
......@@ -36,8 +36,8 @@ class COpendriveRoad
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);
void prune(std::vector<unsigned int> &path_nodes);
bool node_exists_at(TOpendriveWorldPoint &pose);
COpendriveRoadNode* get_node_at(TOpendriveWorldPoint &pose);
bool node_exists_at(TOpendriveWorldPose &pose);
COpendriveRoadNode* get_node_at(TOpendriveWorldPose &pose);
public:
COpendriveRoad();
COpendriveRoad(const COpendriveRoad& object);
......@@ -53,11 +53,11 @@ class COpendriveRoad
unsigned int get_num_nodes(void) const;
const COpendriveRoadNode& get_node(unsigned int index) const;
const COpendriveRoadSegment& operator[](std::size_t index);
unsigned int get_closest_node(TOpendriveWorldPoint &point,double angle_tol=0.1);
double get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
void get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1);
std::vector<unsigned int> get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPoint &start_point,TOpendriveWorldPoint &end_point,COpendriveRoad &new_road);
void get_sub_road(TOpendriveWorldPoint &start_point,double length,COpendriveRoad &new_road);
unsigned int get_closest_node(TOpendriveWorldPose &pose,double angle_tol=0.1);
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1);
void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1);
std::vector<unsigned int> get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road);
void get_sub_road(TOpendriveWorldPose &start_pose,double length,COpendriveRoad &new_road);
void operator=(const COpendriveRoad& object);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
~COpendriveRoad();
......
include/opendrive_road_node.h
View file @ d37ad5b8
......@@ -24,7 +24,7 @@ class COpendriveRoadNode
std::vector<COpendriveLink *> links;
double resolution;
double scale_factor;
TOpendriveWorldPoint point;
TOpendriveWorldPose pose;
std::vector<TOpendriveRoadNodeParent> parents;
unsigned int index;
protected:
......@@ -41,7 +41,7 @@ class COpendriveRoadNode
void set_resolution(double res);
void set_scale_factor(double scale);
void set_index(unsigned int index);
void set_point(TOpendriveWorldPoint &start);
void set_pose(TOpendriveWorldPose &start);
void add_parent(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment);
void add_parent(COpendriveGeometry *geometry,COpendriveLane *lane,COpendriveRoadSegment *segment);
TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane);
......@@ -50,8 +50,8 @@ class COpendriveRoadNode
COpendriveRoadNode *get_original_node(node_up_ref_t &node_refs);
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);
void update_start_pose(COpendriveLane *lane,TOpendriveWorldPoint *start=NULL);
void update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end=NULL);
void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose *start=NULL);
void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose *end=NULL);
public:
double get_resolution(void) const;
double get_scale_factor(void) const;
......@@ -60,14 +60,14 @@ class COpendriveRoadNode
const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
const COpendriveLane &get_parent_lane(unsigned int index) const;
const COpendriveGeometry &get_geometry(unsigned int index) const;
TOpendriveWorldPoint get_point(void) const;
TOpendriveWorldPose get_pose(void) const;
unsigned int get_num_links(void) const;
const COpendriveLink &get_link(unsigned int index) const;
const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
unsigned int get_closest_link(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
double get_closest_distance(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
double get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const;
void get_closest_points(TOpendriveWorldPoint &point,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
unsigned int get_closest_link(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
double get_closest_distance(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
~COpendriveRoadNode();
};
......
include/opendrive_road_segment.h
View file @ d37ad5b8
......@@ -53,7 +53,7 @@ class COpendriveRoadSegment
void remove_lane(COpendriveLane *lane);
void add_nodes(OpenDRIVE::road_type &road_info);
void add_node(const planView::geometry_type &geom_info,COpendriveLane *lane);
void add_empty_node(TOpendriveWorldPoint &point,COpendriveLane *lane);
void add_empty_node(TOpendriveWorldPose &pose,COpendriveLane *lane);
void remove_node(COpendriveRoadNode *node);
bool has_node(COpendriveRoadNode *node);
int get_node_side(COpendriveRoadNode *node);
......@@ -63,7 +63,7 @@ class COpendriveRoadSegment
void link_segment(COpendriveRoadSegment *segment,int from,bool from_start, int to,bool to_start);
bool connects_to(COpendriveRoadSegment *segment);
bool connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2);
COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);
COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref);
public:
std::string get_name(void) const;
......@@ -80,8 +80,8 @@ class COpendriveRoadSegment
const COpendriveObject &get_object(unsigned int index) const;
unsigned int get_num_connecting(void) const;
const COpendriveRoadSegment &get_connecting(unsigned int index) const;
TOpendriveLocalPoint transform_to_local_point(const TOpendriveTrackPoint &track);
TOpendriveWorldPoint transform_to_world_point(const TOpendriveTrackPoint &track);
TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track);
TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
~COpendriveRoadSegment();
};
......
include/opendrive_signal.h
View file @ d37ad5b8
......@@ -15,7 +15,7 @@ class COpendriveSignal
private:
COpendriveRoadSegment *segment;
int id; ///< Signal's id.
TOpendriveTrackPoint pose;
TOpendriveTrackPose pose;
std::string type; ///< Signal's OpenDrive type.
std::string sub_type; ///< Signal's OpenDrive subtype.
int value; ///< Signal's value.
......@@ -30,8 +30,8 @@ class COpendriveSignal
COpendriveSignal(const COpendriveSignal &object);
double get_scale_factor(void);
int get_id(void) const;
TOpendriveTrackPoint get_track_pose(void) const;
TOpendriveWorldPoint get_world_pose(void) const;
TOpendriveTrackPose get_track_pose(void) const;
TOpendriveWorldPose get_world_pose(void) const;
void get_type(std::string &type, std::string &sub_type) const;
int get_value(void) const;
std::string get_text(void) const;
......
include/opendrive_spiral.h
View file @ d37ad5b8
......@@ -12,7 +12,7 @@ class COpendriveSpiral : public COpendriveGeometry
protected:
COpendriveSpiral();
COpendriveSpiral(const COpendriveSpiral &object);
virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
virtual bool transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const;
virtual void print(std::ostream &out);
virtual void load_params(const planView::geometry_type &geometry_info);
virtual std::string get_name(void);
......
src/opendrive_arc.cpp
View file @ d37ad5b8
......@@ -11,7 +11,7 @@ COpendriveArc::COpendriveArc(const COpendriveArc &object) : COpendriveGeometry(o
this->curvature=object.curvature;
}
bool COpendriveArc::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const
bool COpendriveArc::transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
{
double alpha;
......
src/opendrive_geometry.cpp
View file @ d37ad5b8
......@@ -23,11 +23,11 @@ COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
void COpendriveGeometry::print(std::ostream& out)
{
TOpendriveWorldPoint tmp_point=this->get_start_point();
TOpendriveWorldPose tmp_pose=this->get_start_pose();
out << " Geometry " << this->get_name() << std::endl;
out << " lenght: " << this->get_length() << std::endl;
out << " pose: x = " << tmp_point.x << ", y = " << tmp_point.y << ", heading = " << tmp_point.heading << std::endl;
out << " pose: x = " << tmp_pose.x << ", y = " << tmp_pose.y << ", heading = " << tmp_pose.heading << std::endl;
}
void COpendriveGeometry::load(const planView::geometry_type &geometry_info)
......@@ -45,11 +45,11 @@ void COpendriveGeometry::set_scale_factor(double scale)
this->scale_factor=scale;
}
void COpendriveGeometry::set_start_point(TOpendriveWorldPoint &point)
void COpendriveGeometry::set_start_pose(TOpendriveWorldPose &pose)
{
this->pose.x=point.x*this->scale_factor;
this->pose.y=point.y*this->scale_factor;
this->pose.heading=point.heading;
this->pose.x=pose.x*this->scale_factor;
this->pose.y=pose.y*this->scale_factor;
this->pose.heading=pose.heading;
}
void COpendriveGeometry::set_max_s(double s)
......@@ -62,14 +62,14 @@ void COpendriveGeometry::set_min_s(double s)
this->min_s=s*this->scale_factor;
}
bool COpendriveGeometry::get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const
bool COpendriveGeometry::get_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
{
return this->transform_local_pose(track,local);
}
bool COpendriveGeometry::get_world_pose(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world) const
bool COpendriveGeometry::get_world_pose(TOpendriveTrackPose &track,TOpendriveWorldPose &world) const
{
TOpendriveLocalPoint local;
TOpendriveLocalPose local;
if(this->transform_local_pose(track,local))
{
......@@ -105,15 +105,15 @@ double COpendriveGeometry::get_min_s(void) const
return this->min_s/this->scale_factor;
}
TOpendriveWorldPoint COpendriveGeometry::get_start_point(void) const
TOpendriveWorldPose COpendriveGeometry::get_start_pose(void) const
{
TOpendriveWorldPoint tmp_point;
TOpendriveWorldPose tmp_pose;
tmp_point.x=this->pose.x/this->scale_factor;
tmp_point.y=this->pose.y/this->scale_factor;
tmp_point.heading=this->pose.heading;
tmp_pose.x=this->pose.x/this->scale_factor;
tmp_pose.y=this->pose.y/this->scale_factor;
tmp_pose.heading=this->pose.heading;
return tmp_point;
return tmp_pose;
}
void COpendriveGeometry::operator=(const COpendriveGeometry &object)
......
src/opendrive_lane.cpp
View file @ d37ad5b8
......@@ -350,7 +350,7 @@ void COpendriveLane::clean_references(segment_up_ref_t &segment_refs,lane_up_ref
}
}
void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start)
void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *start)
{
std::vector<COpendriveRoadNode *>::iterator it;
segment_up_ref_t new_segment_ref;
......@@ -393,7 +393,7 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
this->nodes[0]->update_start_pose(this,start);
}
void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *end)
void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *end)
{
std::vector<COpendriveRoadNode *>::iterator it;
segment_up_ref_t new_segment_ref;
......@@ -436,7 +436,7 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &
this->nodes[this->nodes.size()-1]->update_end_pose(this,end);
}
COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPoint *start,TOpendriveWorldPoint *end)
COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
{
COpendriveLane *new_lane;
......@@ -638,21 +638,21 @@ int COpendriveLane::get_id(void) const
return this->id;
}
TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
TOpendriveWorldPose COpendriveLane::get_start_pose(void) const
{
TOpendriveTrackPoint track_point;
TOpendriveWorldPoint world_point;
TOpendriveTrackPose track_pose;
TOpendriveWorldPose world_pose;
TOpendriveRoadNodeParent *parent;
std::stringstream error;
track_point.heading=0.0;
track_pose.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;
track_pose.t=this->get_offset()-this->get_width()/2.0;
track_pose.s=0.0;
parent=this->nodes[0]->get_parent_with_lane(this);
if(parent!=NULL)
parent->geometry->get_world_pose(track_point,world_point);
parent->geometry->get_world_pose(track_pose,world_pose);
else
{
error << "Node " << this->nodes[0]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
......@@ -661,12 +661,12 @@ TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
}
else
{
track_point.t=this->get_offset()+this->get_width()/2.0;
track_pose.t=this->get_offset()+this->get_width()/2.0;
parent=this->nodes[0]->get_parent_with_lane(this);
if(parent!=NULL)
{
track_point.s=parent->geometry->get_length();
parent->geometry->get_world_pose(track_point,world_point);
track_pose.s=parent->geometry->get_length();
parent->geometry->get_world_pose(track_pose,world_pose);
}
else
{
......@@ -675,26 +675,26 @@ TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
}
}
if(this->id>0)
world_point.heading=normalize_angle(world_point.heading+3.14159);
world_pose.heading=normalize_angle(world_pose.heading+3.14159);
return world_point;
return world_pose;
}
TOpendriveWorldPoint COpendriveLane::get_end_point(void) const
TOpendriveWorldPose COpendriveLane::get_end_pose(void) const
{
TOpendriveTrackPoint track_point;
TOpendriveWorldPoint world_point;
TOpendriveTrackPose track_pose;
TOpendriveWorldPose world_pose;
TOpendriveRoadNodeParent *parent;
std::stringstream error;
track_point.heading=0.0;
track_pose.heading=0.0;
if(this->id>0)// left lane
{
track_point.t=this->get_offset()+this->get_width()/2.0;
track_point.s=0.0;
track_pose.t=this->get_offset()+this->get_width()/2.0;
track_pose.s=0.0;
parent=this->nodes[this->nodes.size()-1]->get_parent_with_lane(this);
if(parent!=NULL)
parent->geometry->get_world_pose(track_point,world_point);
parent->geometry->get_world_pose(track_pose,world_pose);
else
{
error << "Node " << this->nodes[this->nodes.size()-1]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
......@@ -703,12 +703,12 @@ TOpendriveWorldPoint COpendriveLane::get_end_point(void) const
}
else
{
track_point.t=this->get_offset()-this->get_width()/2.0;
track_pose.t=this->get_offset()-this->get_width()/2.0;
parent=this->nodes[this->nodes.size()-1]->get_parent_with_lane(this);
if(parent!=NULL)
{
track_point.s=parent->geometry->get_length();
parent->geometry->get_world_pose(track_point,world_point);
track_pose.s=parent->geometry->get_length();
parent->geometry->get_world_pose(track_pose,world_pose);
}
else
{
......@@ -717,9 +717,9 @@ TOpendriveWorldPoint COpendriveLane::get_end_point(void) const
}
}
if(this->id>0)
world_point.heading=normalize_angle(world_point.heading+3.14159);
world_pose.heading=normalize_angle(world_pose.heading+3.14159);
return world_point;
return world_pose;
}
double COpendriveLane::get_length(void) const
......@@ -743,15 +743,15 @@ double COpendriveLane::get_length(void) const
return length;
}
TOpendriveLocalPoint COpendriveLane::transform_to_local_point(TOpendriveTrackPoint &track)
TOpendriveLocalPose COpendriveLane::transform_to_local_pose(TOpendriveTrackPose &track)
{
TOpendriveLocalPoint local;
TOpendriveLocalPose local;
TOpendriveRoadNodeParent *parent;
std::stringstream error;
if(track.s<0.0)
{
error << "Invalid track point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
error << "Invalid track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
throw CException(_HERE_,error.str());
}
for(unsigned int i=0;i<this->nodes.size();i++)
......@@ -763,7 +763,7 @@ TOpendriveLocalPoint COpendriveLane::transform_to_local_point(TOpendriveTrackPoi
{
if(!parent->geometry->get_local_pose(track,local))
{
error << "Impossible to transform to local coordinates point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in lane " << parent->lane->get_id() << " of segment " << parent->segment->get_name();
error << "Impossible to transform to local coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in lane " << parent->lane->get_id() << " of segment " << parent->segment->get_name();
throw CException(_HERE_,error.str());
}
return local;
......@@ -778,19 +778,19 @@ TOpendriveLocalPoint COpendriveLane::transform_to_local_point(TOpendriveTrackPoi
}
}
error << "Track point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to lane " << this->id << " of segment " << this->segment->get_name();
error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to lane " << this->id << " of segment " << this->segment->get_name();
throw CException(_HERE_,error.str());
}
TOpendriveWorldPoint COpendriveLane::transform_to_world_point(TOpendriveTrackPoint &track)
TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose &track)
{
TOpendriveWorldPoint world;
TOpendriveWorldPose world;
TOpendriveRoadNodeParent *parent;
std::stringstream error;
if(track.s<0.0)
{
error << "Invalid track point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
error << "Invalid track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
throw CException(_HERE_,error.str());
}
for(unsigned int i=0;i<this->nodes.size();i++)
......@@ -802,7 +802,7 @@ TOpendriveWorldPoint COpendriveLane::transform_to_world_point(TOpendriveTrackPoi
{
if(!parent->geometry->get_world_pose(track,world))
{
error << "Impossible to transform to local coordinates point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in lane " << parent->lane->get_id() << " of segment " << parent->segment->get_name();
error << "Impossible to transform to local coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in lane " << parent->lane->get_id() << " of segment " << parent->segment->get_name();
throw CException(_HERE_,error.str());