diff --git a/include/common.h b/include/common.h new file mode 100644 index 0000000000000000000000000000000000000000..27552d26790b36d8366b4af746258c03c59ba83e --- /dev/null +++ b/include/common.h @@ -0,0 +1,23 @@ +#ifndef _COMMON_H +#define _COMMON_H + +#include <map> +#include <vector> + +class COpendriveRoadSegment; +class COSMRoadSegment; +class CRoad; + +typedef std::pair<COpendriveRoadSegment *,std::vector<CRoad *> > opendrive_map_pair_t; +typedef std::vector<opendrive_map_pair_t> opendrive_road_map_t; + +typedef std::pair<std::vector<CRoad *>,std::vector<COpendriveRoadSegment *> > opendrive_inverse_map_pair_t; +typedef std::vector<opendrive_inverse_map_pair_t> opendrive_road_inverse_map_t; + +typedef std::pair<COSMRoadSegment *,std::vector<CRoad *> > osm_map_pair_t; +typedef std::vector<osm_map_pair_t> osm_road_map_t; + +double normalize_angle(double angle); +double diff_angle(double angle1,double angle2); + +#endif diff --git a/include/g2_spline.h b/include/g2_spline.h index f5e6951c527ea53ca737768e35aa514105a7cdcb..c2faa6c64eb5fc126a8b791f8ec4eb9c0d695a65 100644 --- a/include/g2_spline.h +++ b/include/g2_spline.h @@ -62,14 +62,15 @@ class CG2Spline void get_end_point(TPoint &point); void generate(double resolution,unsigned int iterations=10); void generate(double resolution,double length,unsigned int iterations=10); - TPoint evaluate(double length); + void generate(double &resolution,double n1,double n2, double n3, double n4); + bool evaluate(double length, TPoint& point); void evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading); double get_length(void); double get_max_curvature(double max_error=0.001,unsigned int max_iter=10); double get_max_curvature_der(double max_error=0.001,unsigned int max_iter=10); double find_closest_point(double x,double y,TPoint &point,double max_error=0.001,unsigned int max_iter=10); double find_closest_point(double x,double y,double max_error=0.001,unsigned int max_iter=10); - void get_part(CG2Spline *spline,double start_length, double end_length=-1.0); + bool get_part(CG2Spline *spline,double start_length, double end_length=-1.0); CG2Spline& operator=(const CG2Spline &obj); ~CG2Spline(); }; diff --git a/include/junction.h b/include/junction.h new file mode 100644 index 0000000000000000000000000000000000000000..58ff7efd2392b3d0e9b500a66e95906b1538788f --- /dev/null +++ b/include/junction.h @@ -0,0 +1,93 @@ +#ifndef _JUNCTION_H +#define _JUNCTION_H + +#include "road.h" +#include "road_map.h" +#include "road_segment.h" + +#include <Eigen/Dense> + +class CRoadMap; +class CRoad; +class CRoadSegment; + +class CJunction +{ + friend class CRoad; + friend class CRoadMap; + friend class CRoadSegment; + friend class COpendriveJunction; + private: + unsigned int id; + double resolution; + /* connectivity */ + CRoadMap *parent_roadmap; + std::vector<CRoad *> incomming_roads; + std::vector<CRoad *> outgoing_roads; + Eigen::MatrixXi connectivity; + /* geometry */ + std::vector<CRoadSegment *> segments; + protected: + CJunction(unsigned int id); + void set_id(unsigned int id); + static unsigned int get_index_by_id(const std::vector<CJunction *> &junctions,unsigned int id); + /* connectivity */ + void set_parent_roadmap(CRoadMap *road); + CRoad *get_incomming_road_by_index(unsigned int index); + CRoad *get_outgoing_road_by_index(unsigned int index); + void remove_incomming_road_by_index(unsigned int index); + void remove_outgoing_road_by_index(unsigned int index); + void link_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road); + void unlink_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road); + bool are_roads_linked_by_index(unsigned int incomming_road,unsigned int outgoing_road); + CRoadSegment *link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes); + CRoadSegment *link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,unsigned int outgoing_road); + CRoadSegment *link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,TPoint &outgoing_point,unsigned int outgoing_num_lanes); + void link_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); + void unlink_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); + bool are_lanes_linked_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); + /* geometry */ + CRoadSegment *get_segment_between_by_index(unsigned int incomming_road,unsigned int outgoing_road); + CRoadSegment *get_segment_by_index(unsigned int index); + public: + CJunction(); + unsigned int get_id(void); + void set_resolution(double resolution); + double get_resolution(double resolution); + /* connectivity */ + CRoadMap &get_parent_roadmap(void); + unsigned int get_num_incomming_roads(void); + CRoad &get_incomming_road_by_id(unsigned int id); + unsigned int get_incomming_road_index_by_id(unsigned int id); + bool has_incomming_road(unsigned int id); + unsigned int get_num_outgoing_roads(void); + CRoad &get_outgoing_road_by_id(unsigned int id); + unsigned int get_outgoing_road_index_by_id(unsigned int id); + bool has_outgoing_road(unsigned int id); + void add_incomming_road(CRoad *road); + void remove_incomming_road_by_id(unsigned int id); + void add_outgoing_road(CRoad *road); + void remove_outgoing_road_by_id(unsigned int id); + void link_roads_by_id(unsigned int incomming_road,unsigned int outgoing_road); + void unlink_roads_by_id(unsigned int incomming_road,unsigned int outgoing_road); + bool are_roads_linked_by_id(unsigned int incomming_road,unsigned int outgoing_road); + void link_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); + void link_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road); + void link_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road); + void unlink_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); + void unlink_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road); + void unlink_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road); + bool are_lanes_linked_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); + void get_connectivity_matrix(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &incomming_id_map,std::vector<unsigned int> &outgoing_id_map); + /* geometry */ + unsigned int get_num_segments(void); + CRoadSegment &get_segment_by_id(unsigned int id); + CRoadSegment &get_segment_between_by_id(unsigned int incomming_road,unsigned int outgoing_road); + bool get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max()); + bool get_closest_segment_ids(TPoint &point,std::vector<unsigned int >&segment_ids,std::vector<double >&distances,std::vector<double> &lateral_offsets,double angle_tol=std::numeric_limits<double>::max(),double offset_tol=std::numeric_limits<double>::max()); + void get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw); + void get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw); + ~CJunction(); +}; + +#endif diff --git a/include/opendrive_arc.h b/include/opendrive/opendrive_arc.h similarity index 77% rename from include/opendrive_arc.h rename to include/opendrive/opendrive_arc.h index 683b321b5fe6dc35c67068c0f1bddb4679f26a0a..eecdba3675fe24cfdd95efc5781a0857ec67d422 100644 --- a/include/opendrive_arc.h +++ b/include/opendrive/opendrive_arc.h @@ -15,10 +15,13 @@ class COpendriveArc : public COpendriveGeometry virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const; virtual void print(std::ostream &out); virtual void load_params(const planView::geometry_type &geometry_info); + virtual void save_params(planView::geometry_type &geometry_info); virtual std::string get_name(void); public: + COpendriveArc(TOpendriveWorldPose &start_pose,double length,double curvature); virtual COpendriveGeometry *clone(void); virtual void get_curvature(double &start,double &end); + virtual void get_curvature_at(double length,double &curvature); void operator=(const COpendriveArc &object); ~COpendriveArc(); }; diff --git a/include/opendrive/opendrive_common.h b/include/opendrive/opendrive_common.h new file mode 100644 index 0000000000000000000000000000000000000000..34832ab337f0d6c9bd82a9e6edded1fc53c27518 --- /dev/null +++ b/include/opendrive/opendrive_common.h @@ -0,0 +1,33 @@ +#ifndef _OPENDRIVE_COMMON_H +#define _OPENDRIVE_COMMON_H + +typedef struct +{ + double s; + double t; + double heading; +}TOpendriveTrackPose; + +typedef struct +{ + double u; + double v; + double heading; +}TOpendriveLocalPose; + +typedef struct +{ + double x; + double y; + double heading; +}TOpendriveWorldPose; + +typedef enum{OD_MARK_NONE, + OD_MARK_SOLID, + OD_MARK_BROKEN, + OD_MARK_SOLID_SOLID, + OD_MARK_SOLID_BROKEN, + OD_MARK_BROKEN_SOLID, + OD_MARK_BROKEN_BROKEN} opendrive_mark_t; + +#endif diff --git a/include/opendrive_geometry.h b/include/opendrive/opendrive_geometry.h similarity index 81% rename from include/opendrive_geometry.h rename to include/opendrive/opendrive_geometry.h index f9c7285f1624cd3bf2dff7c2dbeb160f62de9681..50baf13b881b196650c291b0368da3ea77752a6c 100644 --- a/include/opendrive_geometry.h +++ b/include/opendrive/opendrive_geometry.h @@ -1,7 +1,7 @@ #ifndef _OPENDRIVE_GEOMETRY_H #define _OPENDRIVE_GEOMETRY_H -#include "opendrive_common.h" +#include "opendrive/opendrive_common.h" #ifdef _HAVE_XSD #include "xml/OpenDRIVE_1.4H.hxx" #endif @@ -17,29 +17,32 @@ class COpendriveGeometry COpendriveGeometry(); COpendriveGeometry(const COpendriveGeometry &object); void load(const planView::geometry_type &geometry_info); - double scale_factor; + void save(planView::geometry_type &geometry_info); double min_s; ///< Starting track coordenate "s" for the geometry. double max_s; ///< Ending track coordenate "s" for the geometry. TOpendriveWorldPose pose; virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const = 0; virtual void print(std::ostream &out); virtual void load_params(const planView::geometry_type &geometry_info) = 0; + virtual void save_params(planView::geometry_type &geometry_info) = 0; virtual std::string get_name(void)=0; - void set_scale_factor(double scale); void set_start_pose(TOpendriveWorldPose &pose); void set_max_s(double s); void set_min_s(double s); public: + COpendriveGeometry(TOpendriveWorldPose &start_pose,double length); virtual COpendriveGeometry *clone(void) = 0; bool get_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const; bool get_world_pose(const 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; + virtual void get_curvature_at(double length,double &curvature)=0; double get_max_s(void) const; double get_min_s(void) const; TOpendriveWorldPose get_start_pose(void) const; TOpendriveWorldPose get_end_pose(void) const; + bool get_pose_at(double length,TOpendriveWorldPose &pose) const; void operator=(const COpendriveGeometry &object); friend std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom); virtual ~COpendriveGeometry(); diff --git a/include/opendrive/opendrive_junction.h b/include/opendrive/opendrive_junction.h new file mode 100644 index 0000000000000000000000000000000000000000..4a0a03db92888aeaf1c0486cadb510ada6a6d16f --- /dev/null +++ b/include/opendrive/opendrive_junction.h @@ -0,0 +1,47 @@ +#ifndef _OPENDRIVE_JUNCTION_H +#define _OPENDRIVE_JUNCTION_H + +#include "opendrive/opendrive_road_segment.h" + +typedef struct{ + int from_lane_id; + int to_lane_id; +}TJunctionLink; + +typedef struct{ + unsigned int id; + unsigned int incomming_road_id; + unsigned int connecting_road_id; + bool end_contact_point; + std::vector<TJunctionLink> links; +}TJunctionConnection; + +class COpendriveJunction +{ + friend class CRoadMap; + friend class CJunction; + friend class COpendriveRoadSegment; + friend class COpendriveLane; + private: + std::vector<TJunctionConnection> connections; + std::string name; + unsigned int id; + protected: + void set_name(const std::string &name); + void set_id(unsigned int id); + void load(OpenDRIVE::junction_type &junction_info); + void save(OpenDRIVE::junction_type &junction_info); + void convert(CJunction **junction,opendrive_road_map_t &road_map); + static bool get_road_data(unsigned int id,opendrive_road_inverse_map_t &road_map,COpendriveRoadSegment **road,bool incomming); + static void generate_road_segment(CRoadSegment *segment,unsigned int ¤t_id,unsigned int road_in,int lane_in,unsigned int road_out,int lane_out,COpendriveRoadSegment *new_segment); + static void convert(CJunction *junction_in,unsigned int ¤t_id,COpendriveJunction *junction_out,std::vector<COpendriveRoadSegment *> &segments,opendrive_road_inverse_map_t &road_map); + public: + COpendriveJunction(); + std::string get_name(void) const; + unsigned int get_id(void) const; + unsigned int get_num_connections(void) const; + const TJunctionConnection &get_connection(unsigned int index) const; + ~COpendriveJunction(); +}; + +#endif diff --git a/include/opendrive/opendrive_lane.h b/include/opendrive/opendrive_lane.h new file mode 100644 index 0000000000000000000000000000000000000000..959667d6e858e1ba97d477f39047e10b96a80d3e --- /dev/null +++ b/include/opendrive/opendrive_lane.h @@ -0,0 +1,66 @@ +#ifndef _OPENDRIVE_LANE_H +#define _OPENDRIVE_LANE_H + +#ifdef _HAVE_XSD +#include "xml/OpenDRIVE_1.4H.hxx" +#endif + +#include "opendrive/opendrive_common.h" + +class COpendriveLane +{ + friend class COpendriveRoadSegment; + friend class COpendriveJunction; + private: + opendrive_mark_t right_mark; + double speed; + double width; + int id; + protected: + void load(const ::lane &lane_info); + void save(::lane &lane_info); + void set_right_mark(opendrive_mark_t mark); + void set_width(double width); + void set_speed(double speed); + void set_id(int id); + public: + COpendriveLane(); + /** + * \brief return the road mark at the right side of the lane + * + * This function returns the road mark identifier at the right side of the lane, even + * if the right lane is not defined. + * + * \return the road mark identifier at the right side of the lane. + */ + opendrive_mark_t get_right_road_mark(void) const; + /** + * \brief Returns the width of the lane + * + * \returns the width in meters of the lane. + */ + double get_width(void) const; + /** + * \brief Returns the speed limit of the lane + * + * \return the speed limit of the lane in km/h + */ + double get_speed(void) const; + /** + * \brief Returns the opendrive identifier of the lane + * + * This function returns the opendrive identifier of the lane: negative integers + * values are used to identify right lanes, and positive values to identify left + * lanes. These values increase (for left lanes) or decrease (for right lanes) the + * further they are from the center lane of the segment. + * + * \return the opendrive identifier of the lane. + */ + int get_id(void) const; + /** + * brief Destructor + */ + ~COpendriveLane(); +}; + +#endif diff --git a/include/opendrive_line.h b/include/opendrive/opendrive_line.h similarity index 79% rename from include/opendrive_line.h rename to include/opendrive/opendrive_line.h index b544bceed0fd9f0eeb325fd0331a05d25bd97d0f..dce82a0579e7fe2b4a485610bded569a0e30ff85 100644 --- a/include/opendrive_line.h +++ b/include/opendrive/opendrive_line.h @@ -14,10 +14,13 @@ class COpendriveLine : public COpendriveGeometry virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const; virtual void print(std::ostream &out); virtual void load_params(const planView::geometry_type &geometry_info); + virtual void save_params(planView::geometry_type &geometry_info); virtual std::string get_name(void); public: + COpendriveLine(TOpendriveWorldPose &start_pose,double length); virtual COpendriveGeometry *clone(void); virtual void get_curvature(double &start,double &end); + virtual void get_curvature_at(double length,double &curvature); void operator=(const COpendriveLine &object); ~COpendriveLine(); }; diff --git a/include/opendrive_object.h b/include/opendrive/opendrive_object.h similarity index 78% rename from include/opendrive_object.h rename to include/opendrive/opendrive_object.h index 3a0d30f85688623fc5409a17d3cc06dc2ce98587..3e42e1d9c59fbd6cc38e0b797a171ab45ea4ed5f 100644 --- a/include/opendrive_object.h +++ b/include/opendrive/opendrive_object.h @@ -1,11 +1,10 @@ #ifndef _OPENDRIVE_OBJECT_H #define _OPENDRIVE_OBJECT_H -#include "opendrive_common.h" -#include "opendrive_road_segment.h" #ifdef _HAVE_XSD #include "xml/OpenDRIVE_1.4H.hxx" #endif +#include "opendrive/opendrive_common.h" #define OD_MAX_VERTICES 32 @@ -38,30 +37,22 @@ typedef union TOpendrivePolygon polygon; }TOpendriveObject; -class COpendriveRoadSegment; - class COpendriveObject { friend class COpendriveRoadSegment; private: - COpendriveRoadSegment *segment; int id; ///< Object's id. TOpendriveTrackPose pose; TOpendriveObject object; std::string type; ///< Object's OpenDrive type. std::string name; ///< Object's name. object_type_t object_type; - double scale_factor; protected: - void load(objects::object_type &object_info,COpendriveRoadSegment *segment); - void update_references(segment_up_ref_t &segment_refs); - void set_scale_factor(double scale); + void load(objects::object_type &object_info); public: COpendriveObject(); COpendriveObject(const COpendriveObject& object); - double get_scale_factor(void); 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; diff --git a/include/opendrive_param_poly3.h b/include/opendrive/opendrive_param_poly3.h similarity index 75% rename from include/opendrive_param_poly3.h rename to include/opendrive/opendrive_param_poly3.h index c40fdaee54ef4b0f3e4fcb2f0ef23bf99768329b..53fa3aa0525de9bf5ebaadc81d620991c360c703 100644 --- a/include/opendrive_param_poly3.h +++ b/include/opendrive/opendrive_param_poly3.h @@ -25,10 +25,14 @@ class COpendriveParamPoly3 : public COpendriveGeometry virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const; virtual void print(std::ostream &out); virtual void load_params(const planView::geometry_type &geometry_info); + virtual void save_params(planView::geometry_type &geometry_info); virtual std::string get_name(void); public: + COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendrivePoly3Params &u_params,TOpendrivePoly3Params &v_params); + COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose); virtual COpendriveGeometry *clone(void); virtual void get_curvature(double &start,double &end); + virtual void get_curvature_at(double length,double &curvature); TOpendrivePoly3Params get_u_params(void); TOpendrivePoly3Params get_v_params(void); bool is_normalized(void); diff --git a/include/opendrive/opendrive_road_segment.h b/include/opendrive/opendrive_road_segment.h new file mode 100644 index 0000000000000000000000000000000000000000..f6127f19da9bf0da782a705d81bb192c9bbdd689 --- /dev/null +++ b/include/opendrive/opendrive_road_segment.h @@ -0,0 +1,203 @@ +#ifndef _OPENDRIVE_ROAD_SEGMENT_H +#define _OPENDRIVE_ROAD_SEGMENT_H + +#ifdef _HAVE_XSD +#include "xml/OpenDRIVE_1.4H.hxx" +#endif + +#include "road.h" + +#include "opendrive/opendrive_common.h" +#include "opendrive/opendrive_lane.h" +#include "opendrive/opendrive_geometry.h" +#include "opendrive/opendrive_object.h" +#include "opendrive/opendrive_signal.h" + +#include "road_map.h" +#include "road.h" + +typedef struct{ + bool road; + unsigned int id; + bool end_contact; +}TLinkElement; + +class COpendriveRoadSegment +{ + friend class CRoadMap; + friend class CRoad; + friend class COpendriveLane; + friend class COpendriveJunction; + private: + std::vector<COpendriveGeometry *> geometries; + std::vector<COpendriveLane *> right_lanes; + std::vector<COpendriveLane *> left_lanes; + std::vector<COpendriveSignal *> signals; + std::vector<COpendriveObject *> objects; + std::string name; + unsigned int id; + bool predecessor_exists; + TLinkElement predecessor; + bool successor_exists; + TLinkElement successor; + opendrive_mark_t center_mark; + unsigned int junction_id; + protected: + void free(void); + void load(OpenDRIVE::road_type &road_info); + void convert(CRoad **left_road,CRoad **right_road,double resolution); + static void convert(CRoad *left_road,CRoad *right_road,unsigned int ¤t_id,std::vector<COpendriveRoadSegment *> &segments); + void save(OpenDRIVE::road_type **road_info); + void set_name(const std::string &name); + void set_id(unsigned int id); + void set_center_mark(opendrive_mark_t mark); + void add_lanes(lanes::laneSection_type &lane_section); + void add_right_lane(const ::lane &lane_info); + void add_right_lane(COpendriveLane *lane); + void add_left_lane(const ::lane &lane_info); + void add_left_lane(COpendriveLane *lane); + bool add_geometries(OpenDRIVE::road_type &road_info); + void add_geometry(COpendriveGeometry *geometry,double resolution); + void set_junction_id(unsigned int id); + public: + COpendriveRoadSegment(); + /** + * \brief Returns the Opendrive name + * + * \return The Opendrive name of the road segment + */ + std::string get_name(void) const; + /** + * \brief Returns the Opendirve numeric identifier + * + * \return The Opendrive numeric identifier of the road segment. + */ + unsigned int get_id(void) const; + /** + * \brief Returns the number of right lanes + * + * This function returns how many lanes there are on the right side of the road + * segment. The number of left and right lanes can be different. + * + * \return This function returns the number of right lanes. The range of + * valid indexes goes from -1 to the value returned by this function + * negated. + */ + unsigned int get_num_right_lanes(void) const; + /** + * \brief Returns the number of left lanes + * + * This function returns how many lanes there are on the left side of the road + * segment. The number of left and right lanes can be different. + * + * \return This function returns the number of left lanes. The range of + * valid indexes goes from 1 to the value returned by this function. + */ + unsigned int get_num_left_lanes(void) const; + /** + * \brief Returns the number of road signals + * + * This function returns how many road signals are placed at this road segment. + * + * \return This function returns the number of road signals. The range of + * valid indexes goes from 0 to the value returned by this function -1. + */ + unsigned int get_num_signals(void) const; + /** + * \brief Returns a reference to a road signal by local index + * + * This function returns a constant reference to the desired road signal + * object, which can not be modified by the user. If the index is not valid, + * this function will throw an exception. + * + * \param index is the 0 based local index of the desired road signal. + * + * \return constant reference to the desired road signal object. + */ + const COpendriveSignal &get_signal(unsigned int index) const; + /** + * \brief Returns the number of road objects + * + * This function returns how many road objects are placed at this road segment. + * + * \return This function returns the number of road objects. The range of + * valid indexes goes from 0 to the value returned by this function -1. + */ + unsigned int get_num_objects(void) const; + /** + * \brief Returns a reference to a road object by local index + * + * This function returns a constant reference to the desired road object + * object, which can not be modified by the user. If the index is not valid, + * this function will throw an exception. + * + * \param index is the 0 based local index of the desired road object. + * + * \return constant reference to the desired road object object. + */ + const COpendriveObject &get_object(unsigned int index) const; + /** + * \brief Returns true if the road segment is part of a junction, false otherwise + * + * \return a boolean indicating if the road segment is part of a junction or not + */ + bool is_junction(void) const; + /** + * + */ + bool has_predecessor(void); + /** + * + */ + bool is_predecessor_road(void); + /** + * + */ + bool is_predecessor_junction(void); + /** + * + */ + unsigned int get_predecessor_id(void); + /** + * + */ + bool is_predecessor_contact_end(void); + /** + * + */ + bool is_predecessor_contact_start(void); + /** + * + */ + bool has_successor(void); + /** + * + */ + bool is_successor_road(void); + /** + * + */ + bool is_successor_junction(void); + /** + * + */ + unsigned int get_successor_id(void); + /** + * + */ + bool is_successor_contact_end(void); + /** + * + */ + bool is_successor_contact_start(void); + /** + * + */ + double get_length(void); + /** + * \brief Destructor + */ + ~COpendriveRoadSegment(); +}; + +#endif diff --git a/include/opendrive_signal.h b/include/opendrive/opendrive_signal.h similarity index 67% rename from include/opendrive_signal.h rename to include/opendrive/opendrive_signal.h index 5dfe3df686cc39b25424b1e5d2447145b7ab91f2..a57974c6ea3b97c918ef6f36c93f5dff62332bcf 100644 --- a/include/opendrive_signal.h +++ b/include/opendrive/opendrive_signal.h @@ -1,37 +1,29 @@ #ifndef _OPENDRIVE_SIGNAL_H #define _OPENDRIVE_SIGNAL_H -#include "opendrive_common.h" -#include "opendrive_road_segment.h" #ifdef _HAVE_XSD #include "xml/OpenDRIVE_1.4H.hxx" #endif -class COpendriveRoadSegment; +#include "opendrive/opendrive_common.h" class COpendriveSignal { friend class COpendriveRoadSegment; private: - COpendriveRoadSegment *segment; int id; ///< Signal's id. TOpendriveTrackPose pose; std::string type; ///< Signal's OpenDrive type. std::string sub_type; ///< Signal's OpenDrive subtype. int value; ///< Signal's value. std::string text; ///< Signal's text. - double scale_factor; protected: - void load(signals::signal_type &signal_info,COpendriveRoadSegment *segment); - void update_references(segment_up_ref_t &segment_refs); - void set_scale_factor(double scale); + void load(signals::signal_type &signal_info); public: COpendriveSignal(); COpendriveSignal(const COpendriveSignal &object); - double get_scale_factor(void); int get_id(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; diff --git a/include/opendrive_spiral.h b/include/opendrive/opendrive_spiral.h similarity index 76% rename from include/opendrive_spiral.h rename to include/opendrive/opendrive_spiral.h index 106597e8801b0e8f47efa480be43e80c88699eeb..97d4ea8303af5f57647d5d3bc3aaf978b636ab9f 100644 --- a/include/opendrive_spiral.h +++ b/include/opendrive/opendrive_spiral.h @@ -16,10 +16,13 @@ class COpendriveSpiral : public COpendriveGeometry virtual bool transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const; virtual void print(std::ostream &out); virtual void load_params(const planView::geometry_type &geometry_info); + virtual void save_params(planView::geometry_type &geometry_info); virtual std::string get_name(void); public: + COpendriveSpiral(TOpendriveWorldPose &start_pose,double length,double start_curvature,double end_curvature); virtual COpendriveGeometry *clone(void); virtual void get_curvature(double &start,double &end); + virtual void get_curvature_at(double length,double &curvature); void operator=(const COpendriveSpiral &object); ~COpendriveSpiral(); }; diff --git a/include/opendrive_common.h b/include/opendrive_common.h deleted file mode 100644 index da8a28e8cfd42e62aef351fa9efd2cd4347aa73c..0000000000000000000000000000000000000000 --- a/include/opendrive_common.h +++ /dev/null @@ -1,55 +0,0 @@ -#ifndef _OPENDRIVE_COMMON_H -#define _OPENDRIVE_COMMON_H - -#define DEFAULT_RESOLUTION 0.1 -#define DEFAULT_SCALE_FACTOR 1.0 -#define DEFAULT_MIN_ROAD_LENGTH 0.1 - -#include <map> - -class COpendriveLane; -class COpendriveRoadNode; -class COpendriveRoadSegment; -class COpendriveSignal; -class COpendriveObject; -class COpendriveRoad; -class COpendriveLink; - -typedef std::map<COpendriveRoadNode *,COpendriveRoadNode *> node_up_ref_t; -typedef std::map<COpendriveLane *,COpendriveLane *> lane_up_ref_t; -typedef std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> segment_up_ref_t; -typedef std::map<COpendriveLink *,COpendriveLink *> link_up_ref_t; - -typedef struct -{ - double s; - double t; - double heading; -}TOpendriveTrackPose; - -typedef struct -{ - double u; - double v; - double heading; -}TOpendriveLocalPose; - -typedef struct -{ - double x; - double y; - double heading; -}TOpendriveWorldPose; - -typedef enum{OD_MARK_NONE, - OD_MARK_SOLID, - OD_MARK_BROKEN, - OD_MARK_SOLID_SOLID, - OD_MARK_SOLID_BROKEN, - OD_MARK_BROKEN_SOLID, - OD_MARK_BROKEN_BROKEN} opendrive_mark_t; - -double normalize_angle(double angle); -double diff_angle(double angle1,double angle2); - -#endif diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h deleted file mode 100644 index ce8482125e3274303156202ee939649d299e782b..0000000000000000000000000000000000000000 --- a/include/opendrive_lane.h +++ /dev/null @@ -1,382 +0,0 @@ -#ifndef _OPENDRIVE_LANE_H -#define _OPENDRIVE_LANE_H - -#ifdef _HAVE_XSD -#include "xml/OpenDRIVE_1.4H.hxx" -#endif - -#include "opendrive_common.h" -#include "opendrive_road_node.h" -#include "opendrive_road_segment.h" -#include "opendrive_lane.h" - -class COpendriveLane -{ - friend class COpendriveRoadSegment; - friend class COpendriveRoadNode; - friend class COpendriveRoad; - private: - std::vector<COpendriveRoadNode *> nodes; - std::vector<COpendriveLane *> next; - std::vector<COpendriveLane *> prev; - COpendriveLane *left_lane; - opendrive_mark_t left_mark; - COpendriveLane *right_lane; - opendrive_mark_t right_mark; - COpendriveRoadSegment *segment; - double scale_factor; - double resolution; - double width; - double speed; - double offset; - int id; - unsigned int index; - protected: - COpendriveLane(); - COpendriveLane(const COpendriveLane &object); - void load(const ::lane &lane_info,COpendriveRoadSegment *segment); - void link_neightbour_lane(COpendriveLane *lane); - void link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start,bool to_start,bool belongs_to_lane); - void add_next_lane(COpendriveLane *lane); - void add_prev_lane(COpendriveLane *lane); - void remove_lane(COpendriveLane *lane); - void add_node(COpendriveRoadNode *node); - bool has_node(COpendriveRoadNode *node); - bool has_node_with_index(unsigned int index); - void set_parent_segment(COpendriveRoadSegment *segment); - void set_left_lane(COpendriveLane *lane,opendrive_mark_t mark); - void set_right_lane(COpendriveLane *lane,opendrive_mark_t mark); - void set_resolution(double res); - void set_scale_factor(double scale); - void set_width(double width); - void set_speed(double speed); - void set_offset(double offset); - void set_id(int id); - void set_index(unsigned int index); - 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,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start=NULL); - void update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *end=NULL); - COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL); - COpendriveLane *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref); - TOpendriveLocalPose transform_to_local_pose(TOpendriveTrackPose &track) const; - TOpendriveWorldPose transform_to_world_pose(TOpendriveTrackPose &track) const; - public: - /** - * \brief Returns the number of road nodes in the lane - * - * This function returns how many road nodes belong to the lane. - * - * \return This function returns the number of road nodes. The range of - * valid indexes goes from 0 to the value returned by this function -1. - */ - unsigned int get_num_nodes(void) const; - /** - * \brief Returns a reference to a road node by local index - * - * This function returns a constant reference to the desired road node - * object, which can not be modified by the user. If the index is not valid, - * this function will throw an exception. - * - * \param index is the 0 based local index of the desired road node. It is not - * the unique index assigned to the road nodes when they are created. - * - * \return constant reference to the desired road node object. - */ - const COpendriveRoadNode &get_node(unsigned int index) const; - /** - * \brief Returns a reference to the parent road segment - * - * This function returns a constant reference to the road segment where the lane - * belongs, which can not be modified by the user. If the parent segment is not - * defined, this function will throw an exception. - * - * \return constant reference to the parent road segment. - */ - const COpendriveRoadSegment &get_segment(void) const; - /** - * \brief Returns the number of next lanes - * - * This function returns how many lanes are connected to this one in the direction - * of the traffic (the next lanes). - * - * \return This function returns the number of next lanes. The range of - * valid indexes goes from 0 to the value returned by this function -1. - */ - unsigned int get_num_next_lanes(void) const; - /** - * \brief Returns a reference to a next lane - * - * This function returns a constant reference to a next lane connected to this one, - * which can not be modified by the user. If the index is not valid, this function - * will throw an exception. - * - * \param index is the 0 based local index of the desired next lane. It is not - * the unique index assigned to the lanes when they are created. - * - * \return constant reference to a next lane. - */ - const COpendriveLane &get_next_lane(unsigned int index) const; - /** - * \brief Returns the number of previous lanes - * - * This function returns how many lanes are connected to this one in the opposite - * direction of the traffic (the previous lanes). - * - * \return This function returns the number of previous lanes. The range of - * valid indexes goes from 0 to the value returned by this function -1. - */ - unsigned int get_num_prev_lanes(void) const; - /** - * \brief Returns a reference to a previous lane - * - * This function returns a constant reference to a previous lane connected to this one, - * which can not be modified by the user. If the index is not valid, this function - * will throw an exception. - * - * \param index is the 0 based local index of the desired previous lane. It is not - * the unique index assigned to the lanes when they are created. - * - * \return constant reference to a previous lane. - */ - const COpendriveLane &get_prev_lane(unsigned int index) const; - /** - * \brief Checks whether there exist a left lane or not - * - * This function checks if the lane has an adjacent left lane. Adjacent lanes are - * on the same side of the road. Lanes of opposite sides of the road are not - * considered adjacent. - * - * \return true if the lane has an adjacent left lane or false otherwise. - */ - bool exist_left_lane(void); - /** - * \brief Returns a reference to a left lane - * - * This function returns a constant reference to a left lane adjacent to this one, - * which can not be modified by the user. If there is no adjacent left lane, this - * function will throw an exception. - * - * \return constant reference to the left lane. - */ - const COpendriveLane &get_left_lane(void) const; - /** - * \brief Checks whether there exist a right lane or not - * - * This function checks if the lane has an adjacent right lane. Adjacent lanes are - * on the same side of the road. Lanes of opposite sides of the road are not - * considered adjacent. - * - * \return true if the lane has an adjacent right lane or false otherwise. - */ - bool exist_right_lane(void); - /** - * \brief Returns a reference to a right lane - * - * This function returns a constant reference to a right lane adjacent to this one, - * which can not be modified by the user. If there is no adjacent right lane, this - * function will throw an exception. - * - * \return constant reference to the right lane. - */ - const COpendriveLane &get_right_lane(void) const; - /** - * \brief return the road mark at the left side of the lane - * - * This function returns the road mark identifier at the left side of the lane, even - * if the left lane is not defined. - * - * \return the road mark identifier at the left side of the lane. - */ - opendrive_mark_t get_left_road_mark(void) const; - /** - * \brief return the road mark at the right side of the lane - * - * This function returns the road mark identifier at the right side of the lane, even - * if the right lane is not defined. - * - * \return the road mark identifier at the right side of the lane. - */ - opendrive_mark_t get_right_road_mark(void) const; - /** - * \brief Returns the width of the lane - * - * \returns the width in meters of the lane. - */ - double get_width(void) const; - /** - * \brief Returns the speed limit of the lane - * - * \return the speed limit of the lane in km/h - */ - double get_speed(void) const; - /** - * \brief Returns the offset from the road segment center. - * - * This function returns the offset of the center of the lane with respect of - * the center of the road segment where it belongs. - * - * \return the distance form the center of the segment in meters. - */ - double get_center_offset(void) const; - /** - * \brief Returns the unique index of the lane - * - * \return the unique index assigned to the lane when it was created. - */ - unsigned int get_index(void) const; - /** - * \brief Returns the opendrive identifier of the lane - * - * This function returns the opendrive identifier of the lane: negative integers - * values are used to identify right lanes, and positive values to identify left - * lanes. These values increase (for left lanes) or decrease (for right lanes) the - * further they are from the center lane of the segment. - * - * \return the opendrive identifier of the lane. - */ - int get_id(void) const; - /** - * \brief Returns the start pose of the lane - * - * This function returns the start pose (x and y coordinates and heading) of the - * lane in the world reference system. The lanes are defined in the direction of - * the traffic. That is, right lanes have the same heading as the road segment, - * but left lanes have opposite heading, starting at the end of the road segment - * and ending at the start. - * - * \return the start pose of the lane in world coordinates. - */ - TOpendriveWorldPose get_start_pose(void) const; - /** - * \brief Returns the end pose of the lane - * - * This function returns the end pose (x and y coordinates and heading) of the - * lane in the world reference system. The lanes are defined in the direction of - * the traffic. That is, right lanes have the same heading as the road segment, - * but left lanes have opposite heading, starting at the end of the road segment - * and ending at the start. - * - * \return the end pose of the lane in world coordinates. - */ - TOpendriveWorldPose get_end_pose(void) const; - /** - * \brief returns the length of the lane - * - * This function returns the length of the lane geometry. The lane geometry is - * the same as the link geometries of the lane links connecting the road nodes - * belonging to the lane. - * - * \return length of the lane in meters. - */ - double get_length(void) const; - /** - * \brief returns the pose of the lane at a given point - * - * This function returns the pose (x and y coordinates and heading) of the lane geometry - * at a given distance from the origin point of the lane. The lane geometry is - * the same as the link geometries of the lane links connecting the road nodes - * belonging to the lane. - * - * \param length desired distance from the origin point of the lane in meters. This - * distance must be smaller or equal than the actual lane length. - * - * \return the pose at the desired point in the world reference system. - */ - TOpendriveWorldPose get_pose_at(double length) const; - /** - * \brief returns the curvature of the lane at a given point - * - * This function returns the curvature of the lane geometry at a given distance from - * the origin point of the lane. The lane geometry is the same as the link geometries - * of the lane links connecting the road nodes belonging to the lane. - * - * \param length desired distance from the origin point of the lane in meters. This - * distance must be smaller or equal than the actual lane length. - * - * \return the curvature at the desired point. - */ - double get_curvature_at(double length) const; - /** - * \brief returns the closest node to a given pose - * - * This functions finds the closest road node belonging to the lane to a given - * pose, and returns it. This function only returns a valid reference when the - * given pose coincides with the lane. Otherwise, it throws an exception. - * - * \param world given pose to find the closest road node. - * \param distance the node distance between the projection of the given pose to - * the lane link geometry of the closest road node, and the start - * position of the node, following the corresponding lane link geometry. - * If the closest road node does not exist this argument is set to the - * maximum double value (std::numeric_limits<double>::max()) - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane link geometry of the closest road node - * - * \return a constant reference to the closest node which can not be modified by the - * user. - */ - const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the index of the closest node to a given pose - * - * This functions finds the closest road node belonging to the lane to a given - * pose, and returns its index. This function only returns a valid index - * when the given pose coincides with the lane. - * - * \param world given pose to find the closest road node. - * \param distance the node distance between the projection of the given pose to - * the lane link geometry of the closest road node, and the start - * position of the node, following the corresponding lane link geometry. - * If the closest road node does not exist this argument is set to the - * maximum double value (std::numeric_limits<double>::max()) - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane link geometry of the closest road node - * - * \return the index of the closest node or -1 if there is no node close to the given - * pose. - */ - unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the closest pose to a given pose - * - * This functions finds the closest pose (minimum distance) on the lane geometry - * to a given pose. The closest pose only exist if the imaginary line between the - * given pose and its projection into the lane is orthogonal to the lane geometry. - * Otherwise, it is considered that the given pose does not coincide with the lane. - * - * \param world given pose to find the closest pose. - * \param closest_pose the closest pose to the lane if the given pose coincides with - * it. Otherwise the data on this parameters is not valid. - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane geometry. - * - * \return the lane distance between the projection of the given pose to the - * lane geometry, following its geometry. If the closest pose does not exist, - * the maximum double value (std::numeric_limits<double>::max()) will be - * returned. - */ - double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const; - /** - * \brief overloaded stream operator - * - * This functions streams human readable information about the lane. This information - * includes: - * * The opendrive lane id. - * * Its width, sped limit and offset from the center of the road segment. - * * The parent road segment. - * * For each previous and next lanes: the road segment and lane information. - * * The left lane and the left road mark. - * * The right lane and the right road mark. - * * The road nodes information. - * - */ - friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane); - /** - * brief Destructor - */ - ~COpendriveLane(); -}; - -#endif diff --git a/include/opendrive_link.h b/include/opendrive_link.h deleted file mode 100644 index bca711ab7cf501546230eba3f71e9c4a72921648..0000000000000000000000000000000000000000 --- a/include/opendrive_link.h +++ /dev/null @@ -1,223 +0,0 @@ -#ifndef _OPENDRIVE_LINK_H -#define _OPENDRIVE_LINK_H - -#include "opendrive_common.h" -#include "g2_spline.h" -#include "opendrive_line.h" -#include "opendrive_spiral.h" -#include "opendrive_arc.h" -#include "opendrive_param_poly3.h" -#include "opendrive_road_node.h" - -class COpendriveLink -{ - friend class COpendriveRoadSegment; - friend class COpendriveRoadNode; - friend class COpendriveRoad; - private: - COpendriveRoadNode *prev; - COpendriveRoadNode *next; - COpendriveRoadSegment *segment; - COpendriveLane *lane; - CG2Spline *spline; - opendrive_mark_t mark; - double resolution; - double scale_factor; - protected: - COpendriveLink(); - COpendriveLink(const COpendriveLink &object); - void set_prev(COpendriveRoadNode *node); - void set_next(COpendriveRoadNode *node); - void set_road_mark(opendrive_mark_t mark); - void set_parent_segment(COpendriveRoadSegment *segment); - void set_parent_lane(COpendriveLane *lane); - void set_resolution(double res); - void set_scale_factor(double scale); - void generate(double start_curvature,double end_curvature); - void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs); - bool clean_references(node_up_ref_t &refs); - void update_start_pose(TOpendriveWorldPose &start,double curvature); - void update_end_pose(TOpendriveWorldPose &end,double curvature); - public: - /** - * \brief Returns a reference to the previous road node - * - * This function returns a constant reference to the previous COpendriveRoadNode - * object. If the previous road node is not defines, an exception will be thrown. - * - * \return a constant reference to the previous COpendriveRoadNode object. - */ - const COpendriveRoadNode &get_prev(void) const; - /** - * \brief Returns a reference to the next road node - * - * This function returns a constant reference to the next COpendriveRoadNode - * object. If the next road node is not defines, an exception will be thrown. - * - * \return a constant reference to the next COpendriveRoadNode object. - */ - const COpendriveRoadNode &get_next(void) const; - /** - * \brief Returns the road mark identifier between the two road nodes - * - * This function returns the possible road mark between the two COpendriveRoadNode - * objects linked by each instance of this class. For lane links this will always - * be OD_MARK_NONE, but for non-lane links it will correspond to the actual road - * mark. - * - * \return the road mark identifier. - */ - opendrive_mark_t get_road_mark(void) const; - /** - * \brief Returns the reference to the parent road segment - * - * This function returns a constant reference to the COpendriveRoadSegment object - * where the each instance of this class belongs. If the reference is not valid, - * an exception will be thrown. - * - * \return a constant reference to the parent COpendriveRoadSegment object. - */ - const COpendriveRoadSegment &get_parent_segment(void) const; - /** - * \brief Returns the reference to the parent lane - * - * This function returns a constant reference to the COpendriveLane object - * where the each instance of this class belongs. This function will fail for - * non-lane links since they do not have a parent lane. In this case an - * exception will be thrown. - * - * Use the is_lane_link() function to check the type of link. - * - * \return a constant reference to the parent COpendriveRoadSegment object. - */ - const COpendriveLane &get_parent_lane(void) const; - /** - * \brief returns the type of link - * - * This function returns whether each instance of this class is a lane link - * or not. - * - * \return true if the instance is a lane link and false otherwise. - */ - bool is_lane_link(void) const; - /** - * \brief Returns the resolution - * - * \return This function returns the current resolution - */ - double get_resolution(void) const; - /** - * \brief Returns the scale factor - * - * \return This function returns the current scale factor - */ - double get_scale_factor(void) const; - /** - * \brief returns the closest pose to a given one - * - * This functions finds the closest pose (minimum distance) on the link geometry - * to a given position. The closest pose only exist if the imaginary line between the - * given pose and its projection into the link is orthogonal to the link geometry. - * Otherwise, it is considered that the given pose does not coincide with the link - * object. - * - * \param world given pose to find the closest point. The heading information is - * ignored. - * \param pose the closest pose on the link geometry if the given pose coincide - * with the link object. Otherwise the data on this parameters is not - * valid. - * - * \return the link distance between the projected pose and the link start position, - * following the link geometry, if given pose coincide with the link object. - * Otherwise the maximum double value (std::numeric_limits<double>::max()) - * will be returned. - */ - double find_closest_world_pose(TOpendriveWorldPose &world,TOpendriveWorldPose &pose) const; - /** - * \brief returns the link trajectory (only position) - * - * This function returns the x and y coordinates of the link geometry evaluated at - * the internal resolution. By default the whole trajectory is returned, but is - * possible to get only a sub-trajectory by specifying the start and end lengths. - * - * \param x a vector with the x coordinates of the link trajectory (sub-trajectory) - * in the world reference system. - * \param y a vector with the y coordinates of the link trajectory (sub-trajectory) - * in the world reference system. - * \param start_length initial distance from the origin position of the link in - * meters. By default this value is 0.0. - * \param end_length final distance from the origin position of the link in meters. - * By default this value is -1.0 to indicate the end of the link. - */ - void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const; - /** - * \brief returns the link trajectory (position and heading) - * - * This function returns the x and y coordinates, as well as the heading of the - * link geometry evaluated at the internal resolution. By default the whole - * trajectory is returned, but is possible to get only a sub-trajectory by - * specifying the start and end lengths. - * - * \param x a vector with the x coordinates of the link trajectory (sub-trajectory) - * in the world reference system. - * \param y a vector with the y coordinates of the link trajectory (sub-trajectory) - * in the world reference system. - * \param yaw a vector with the heading of the link trajectory (sub-trajectory) - * in the world reference system. - * \param start_length initial distance from the origin position of the link in - * meters. By default this value is 0.0. - * \param end_length final distance from the origin position of the link in meters. - * By default this value is -1.0 to indicate the end of the link. - */ - 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; - /** - * \brief returns the length of the link - * - * This function returns the length of the link geometry. - * - * \return length of the link geometry in meters. - */ - double get_length(void) const; - /** - * \brief returns the pose of the link at a given point - * - * This function returns the pose (x and y coordinates and heading) of the link geometry - * at a given distance from the origin point of the link. - * - * \param length desired distance from the origin point of the link in meters. This - * distance must be smaller or equal than the actual link length. - * - * \return the pose at the desired point in the world reference system. - */ - TOpendriveWorldPose get_pose_at(double length); - /** - * \brief returns the curvature of the link at a given point - * - * This function returns the curvature of the link geometry at a given distance from - * the origin point of the link. - * - * \param length desired distance from the origin point of the link in meters. This - * distance must be smaller or equal than the actual link length. - * - * \return the curvature at the desired point. - */ - double get_curvature_at(double length); - /** - * \brief overloaded stream operator - * - * This functions streams human readable information about the link. This information - * includes: - * * Index of the previous and next COpendriveRoad node objects. - * * Name of the parent COpendriveROadSegment object. - * * Id of the parent lane for lane links. - * * The road mark between the two road nodes. - * - */ - friend std::ostream& operator<<(std::ostream& out, COpendriveLink &link); - /** - * \brief Destructor - */ - ~COpendriveLink(); -}; - -#endif diff --git a/include/opendrive_road.h b/include/opendrive_road.h deleted file mode 100644 index 4d3966e90a859c8d64739ecd88f2a4a44a140957..0000000000000000000000000000000000000000 --- a/include/opendrive_road.h +++ /dev/null @@ -1,430 +0,0 @@ -#ifndef _OPENDRIVE_ROAD_H -#define _OPENDRIVE_ROAD_H - -#ifdef _HAVE_XSD -#include "xml/OpenDRIVE_1.4H.hxx" -#endif - -#include "opendrive_common.h" -#include "opendrive_road_segment.h" -#include "opendrive_road_node.h" -#include "opendrive_lane.h" - -class COpendriveRoad -{ - friend class COpendriveRoadSegment; - friend class COpendriveRoadNode; - friend class COpendriveLane; - private: - std::vector<COpendriveRoadSegment *> segments; - std::vector<COpendriveRoadNode *> nodes; - std::vector<COpendriveLane *> lanes; - double resolution; - double scale_factor; - double min_road_length; - protected: - void free(void); - void link_segments(OpenDRIVE &open_drive); - unsigned int add_node(COpendriveRoadNode *node); - bool remove_node(COpendriveRoadNode *node); - COpendriveRoadNode* get_node_at(TOpendriveWorldPose &pose); - bool node_exists_at(TOpendriveWorldPose &pose); - unsigned int add_lane(COpendriveLane *lane); - bool remove_lane(COpendriveLane *lane); - void complete_open_lanes(void); - void add_segment(COpendriveRoadSegment *segment); - bool has_segment(COpendriveRoadSegment *segment); - std::vector<unsigned int> update_path(node_up_ref_t &node_refs,std::vector<unsigned int> &path); - 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 reindex(void); - void prune(std::vector<unsigned int> &path_nodes); - public: - /** - * \brief Default constructor - */ - COpendriveRoad(); - /** - * \brief Copy constructor - * - * \param object constant reference to the object to be copied - */ - COpendriveRoad(const COpendriveRoad& object); - /** - * \brief Load an Opendrive road description - * - * This function loads the Opendrive description file initializes the internal - * data structures. If the file referenced by the parameter does not exist or - * the format is not valid, this function will throw an exception. - * - * \param filename the name of the opendrive file to load with the extension - * and the full path. - */ - void load(const std::string &filename); - /** - * \brief Sets the resolution - * - * This functions sets the default resolution used to generate all the internal - * geometry. This functions automatically changes the resolution of all underlying - * objects. - * - * \param res desired value of the resolution in meters. The default resolution - * is 0.1 m. - */ - void set_resolution(double res); - /** - * \brief Sets the scale factor - * - * This function sets the scale factor to be applied to all the internal geometry. - * The scale factor makes it possible to change the size of the road without actually - * modifying the internal geometry data. This functions automatically changes - * the scale factor of all underlying objects. - * - * \param scale desired scale factor. A value bigger than 1.0 will scale down the - * road and a value smaller than 1.0 will scale it up. The default - * value is 1.0. - */ - void set_scale_factor(double scale); - /** - * \brief Sets the minim road length - * - * This function sets the length threshold to include any imported road segments into - * the internal structure. This functions automatically changes the minimum road length - * of all underlying objects. - * - * \param length threshold value to include a road segment. The default value is 0.01. - */ - void set_min_road_length(double length); - /** - * \brief Returns the resolution - * - * \return This function returns the current resolution - */ - double get_resolution(void) const; - /** - * \brief Returns the scale factor - * - * \return This function returns the current scale factor - */ - double get_scale_factor(void) const; - /** - * \brief Returns the minimum road length - * - * \return This function returns the current minimum road length - */ - double get_min_road_length(void) const; - /** - * \brief Returns the number of road segment objects in the whole road - * - * \return This function returns the number of road segments present in - * whole road. The range of valid indexes goes from 0 to the - * value returned by this function -1. - */ - unsigned int get_num_segments(void) const; - /** - * \brief Returns a road segment by index - * - * This function returns a constant reference to the desired road segment - * object, which can not be modified by the user. If the index is not valid, - * this function will throw an exception. - * - * \param index is the 0 based index of the desired road segment - * - * \return constant reference to the desired road segment object. - */ - const COpendriveRoadSegment& get_segment(unsigned int index) const; - /** - * \brief Returns a road segment by id - * - * This function returns a constant reference to the desired road segment - * object, which can not be modified by the user. If the id is not valid, - * this function will throw an exception. - * - * \param id is the Opendrive numeric id of the road segment. - * - * \return constant reference to the desired road segment object. - */ - const COpendriveRoadSegment &get_segment_by_id(unsigned int id) const; - /** - * \brief Returns a road segment by name - * - * This function returns a constant reference to the desired road segment - * object, which can not be modified by the user. If the name is not valid, - * this function will throw an exception. - * - * \param name is the Opendrive name of the road segment. - * - * \return constant reference to the desired road segment object. - */ - const COpendriveRoadSegment &get_segment_by_name(std::string &name) const; - /** - * \brief Returns the number of lane objects in the whole road - * - * \return This function returns the number of lanes present in whole - * road. The range of valid indexes goes from 0 to the value - * returned by this function -1. - */ - unsigned int get_num_lanes(void) const; - /** - * \brief Returns a lane by index - * - * This function returns a constant reference to the desired lane - * object, which can not be modified by the user. If the index is not valid, - * this function will throw an exception. - * - * \param index is the 0 based index of the desired lane - * - * \return constant reference to the desired lane object. - */ - const COpendriveLane &get_lane(unsigned int index) const; - /** - * \brief Returns the number of road node objects in the whole road - * - * \return This function returns the number of road nodes present in whole - * road. The range of valid indexes goes from 0 to the value - * returned by this function -1. - */ - unsigned int get_num_nodes(void) const; - /** - * \brief Returns a road node by index - * - * This function returns a constant reference to the desired road node - * object, which can not be modified by the user. If the index is not valid, - * this function will throw an exception. - * - * \param index is the 0 based index of the desired road node - * - * \return constant reference to the desired road node object. - */ - const COpendriveRoadNode& get_node(unsigned int index) const; - /** - * \brief overloaded index operator - * - * This operator is similar to the get_segment() function. It returns a - * constant reference to the desired road segment object, which can not - * be modified by the user. If the index is not valid,this function will - * throw an exception. - * - * \param index is the 0 based index of the desired road segment - * - * \return constant reference to the desired road segment object. - */ - const COpendriveRoadSegment &operator[](std::size_t index); - /** - * \brief returns the closest node to a given pose - * - * This functions finds the closest road node belonging to the whole road to a - * given pose, and returns it. - * - * \param world given pose to find the closest road node. - * \param distance the node distance between the projection of the given pose to - * the lane link geometry of the closest road node, and the start - * position of the node, following the corresponding lane link geometry. - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane link geometry of the closest road node. - * - * \return a constant reference to the closest node which can not be modified by the - * user. - */ - const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the index of the closest node to a given pose - * - * This functions finds the closest road node belonging to the whole road to a given - * pose, and returns its index. - * - * \param world given pose to find the closest road node. - * \param distance the node distance between the projection of the given pose to - * the lane link geometry of the closest road node, and the start - * position of the node, following the corresponding lane link geometry. - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane link geometry of the closest road node. - * - * \return the index of the closest road node. - */ - unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the closest lane to a given pose - * - * This functions finds the closest lane belonging to the whole road to a - * given pose, and returns it. - * - * \param world given pose to find the closest lane. - * \param distance the lane distance between the projection of the given pose to - * the lane geometry, and the start position of the lane, following - * its geometry. - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane geometry. - * - * \return a constant reference to the closest lane which can not be modified by the - * user. - */ - const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the index of the closest lane to a given pose - * - * This functions finds the closest lane belonging to the whole road to a given - * pose, and returns its index. - * - * \param world given pose to find the closest lane. - * \param distance the lane distance between the projection of the given pose to - * the lane geometry, and the start position of the lane, following - * its geometry. - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane geometry. - * - * \return the index of the closest lane. - */ - unsigned int get_closest_lane_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the closest road segment to a given pose - * - * This functions finds the closest road segment belonging to the whole road to a - * given pose, and returns it. The geometry used to find the closest road segment is the - * one that describes the center lane of the segment, not the lane/link geometries. - * - * \param world given pose to find the closest road segment. - * \param distance the segment distance between the projection of the given pose to - * the road segment geometry, and the start position of the segment, following - * its geometry. - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the road segment geometry. - * - * \return a constant reference to the closest road segment which can not be modified by the - * user. - */ - const COpendriveRoadSegment &get_closest_segment(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the index of the closest road segment to a given pose - * - * This functions finds the closest road segment belonging to the whole road to a given - * pose, and returns its index. The geometry used to find the closest road segment is the - * one that describes the center lane of the segment, not the lane/link geometries. - * - * \param world given pose to find the closest road segment. - * \param distance the segment distance between the projection of the given pose to - * the road segment geometry, and the start position of the segment, following - * its geometry. - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the road segment geometry. - * - * \return the index of the closest road segment. - */ - unsigned int get_closest_segment_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the closest pose - * - * This functions finds the closest pose (minimum distance) to the whole road - * to a given pose. - * - * \param world given pose to find the closest pose. - * \param closest_pose the closest pose to the closest to any road geometry. - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on any road geometry. - * - * \return the link distance between the projection of the given pose to the - * closest road geometry, following its geometry. - */ - double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const; - /** - * \brief Returns a set of poses on the whole road close to a given pose - * - * This function is similar to the get_closest_pose(), but in this case, the closest - * pose of any road geometry which is closer to the given pose that a given distance will be - * returned. - * - * \param world given pose to find the closest poses. - * \param closest_poses the closest poses on any road geometry. - * \param distances the link distance between the projection of the given pose to each - * of the closest road geometries, following the corresponding geometry. - * \param dist_tol maximum allowed distance from the given position to any road geometry. - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on any road geometry. - */ - void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &distances,double dist_tol,double angle_tol=0.1) const; - /** - * \brief creates a sub-road with only the road elements of a given path - * - * This functions creates a new sub road with only the whole road segments that - * include the road nodes of the given path. These new road segments will only have - * the lanes and the road nodes on the same side as the given path. - * - * The road elements of the new road keep the required original information and connectivity - * but are completely independent from the original ones. The only information that - * changes in the new sub-road is: - * - * * the remaining road segments, lanes and road nodes are re-indexed, so that their - * unique index is coherent with the new data structure. - * * for any open road segment (that is, a road segment that is not connected), new road - * nodes are placed at the end of the open road segment to keep its lane and link - * geometric information. - * - * If the given path has non-consecutive road nodes, this function will fail throwing an - * exception. - * - * \param path_nodes sequence of road nodes describing the path. The road nodes are - * identified by their unique index. - * \param end_pose end pose of the path. When final road node has multiple road segment - * parents, the end pose indicates which one of them must be included - * in the new road. - * \param new_road The new sub-road object. - * - * \return the sequence of road nodes describing the path in the new sub-road. - */ - std::vector<unsigned int> get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road); - /** - * \brief creates a sub-road with only the road elements within a distance to a given pose - * - * This functions creates a new sub road stating a given distance (margin) from the start - * pose and reaching up to a given distance following the traffic direction of the start - * pose. The new road segments will keep all the original lanes and road nodes. - * - * The distance is measured following the road segment geometries, not the euclidean - * distance. The new road may be shorter than expected if the original road is not long - * enough. - * - * The road elements of the new road keep the required original information and connectivity - * but are completely independent from the original ones. The only information that - * changes in the new sub-road is: - * - * * the remaining road segments, lanes and road nodes are re-indexed, so that their - * unique index is coherent with the new data structure. - * * for any open road segment (that is, a road segment that is not connected), new road - * nodes are placed at the end of the open road segment to keep its lane and link - * geometric information. - * - * If the part of the road of interest has multiple paths, this function will fail throwing an - * exception. - * - * \param start_pose start pose of the new road. This pose implicitly defines the side - * of the road taken to generate the sub-road. - * \param length this is the desired length of the new road in meters. On success, this - * parameter will be updated with the actual length of the new road. - * \param margin a distance to be added at the beginning and the end of the new road, if - * possible. This makes the actual length of the new road equal to length+ - * 2.0*margin. - * \param new_road The new sub-road object. - * - */ - void get_sub_road(TOpendriveWorldPose &start_pose,double &length,double margin,COpendriveRoad &new_road); - /** - * \brief creates a full copy of the road - */ - void operator=(const COpendriveRoad& object); - /** - * \brief overloaded stream operator - * - * This functions streams human readable information about the whole road. This information - * includes: - * * The resolution, scale_factor and minimum road length - * * The road segments information. - * - */ - friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road); - /** - * \brief Destructor - */ - ~COpendriveRoad(); -}; - -#endif diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h deleted file mode 100644 index b5d0aae3fbdb753b79e62bafe3ae0b8979f13bb1..0000000000000000000000000000000000000000 --- a/include/opendrive_road_node.h +++ /dev/null @@ -1,305 +0,0 @@ -#ifndef _OPENDRIVE_ROAD_NODE_H -#define _OPENDRIVE_ROAD_NODE_H - -#include <vector> -#include "opendrive_common.h" -#include "opendrive_link.h" -#include "opendrive_lane.h" -#include "opendrive_road_segment.h" - -typedef struct -{ - COpendriveLane * lane; - COpendriveRoadSegment *segment; - double start_curvature; - double end_curvature; -}TOpendriveRoadNodeParent; - -class COpendriveRoadNode -{ - friend class COpendriveLane; - friend class COpendriveRoadSegment; - friend class COpendriveRoad; - friend class COpendriveLink; - private: - std::vector<COpendriveLink *> links; - double resolution; - double scale_factor; - TOpendriveWorldPose pose; - std::vector<TOpendriveRoadNodeParent> parents; - unsigned int index; - protected: - COpendriveRoadNode(); - COpendriveRoadNode *clone(link_up_ref_t &new_link_ref); - void free(void); - bool add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane); - void add_link(COpendriveLink *link); - void remove_link(COpendriveLink *link); - bool has_link(COpendriveLink *link); - COpendriveLink *get_link_with(COpendriveRoadNode *node); - void set_resolution(double res); - void set_scale_factor(double scale); - void set_index(unsigned int index); - void set_pose(TOpendriveWorldPose &start); - void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double start_curvature,double end_curvature); - TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane); - TOpendriveRoadNodeParent *get_parent_with_segment(const COpendriveRoadSegment *segment); - bool updated(node_up_ref_t &refs); - 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,TOpendriveWorldPose &start,double distance); - void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance); - public: - /** - * \brief Returns the resolution - * - * \return This function returns the current resolution - */ - double get_resolution(void) const; - /** - * \brief Returns the scale factor - * - * \return This function returns the current scale factor - */ - double get_scale_factor(void) const; - /** - * \brief returns the node index - * - * This function returns the unique node index which identified the road node object. - * This index is assigned when the Opendrive file is loaded and may only change when - * a new sub-road is generated, in which case the road nodes in the new road are re- - * indexed. - * - * \return the index of the road node. - */ - unsigned int get_index(void) const; - /** - * \brief Returns the number of parents - * - * This function returns how many parents the road node have. Each parent is identified - * by the road segment and also the lane where it belongs. - * - * \return This function returns the number of parent of the road node. The range of - * valid indexes goes from 0 to the value returned by this function -1. - */ - unsigned int get_num_parents(void) const; - /** - * \brief Returns a reference to a road segment parent - * - * This function returns a constant reference to a COpendriveRoadSegment object which - * is a parent of the road node. This reference can not be modified by the user. - * If the index is not valid, this function will throw an exception. - * - * \param index is the 0 based index of the desired parent - * - * \return constant reference to the desired road segment parent object. - */ - const COpendriveRoadSegment& get_parent_segment(unsigned int index) const; - /** - * \brief Returns a reference to a lane parent - * - * This function returns a constant reference to a COpendriveLane object which - * is a parent of the road node. This reference can not be modified by the user. - * If the index is not valid, this function will throw an exception. - * - * \param index is the 0 based index of the desired parent - * - * \return constant reference to the desired lane parent object. - */ - const COpendriveLane &get_parent_lane(unsigned int index) const; - /** - * \brief returns the position of the road node - * - * This function returns the pose of the road node in the world reference system. - * - * \return world pose of the road node. - */ - TOpendriveWorldPose get_pose(void) const; - /** - * \brief Returns the number of links - * - * This function returns how many links are connecting to this road node, either - * starting or ending at it. - * - * \return This function returns the number of links of the road node. The range of - * valid indexes goes from 0 to the value returned by this function -1. - */ - unsigned int get_num_links(void) const; - /** - * \brief Returns a reference to a link - * - * This function returns a constant reference to a COpendriveLink object which - * connects this node to another one. In this case both links starting and ending - * at the desired road node can be accessed. This reference can not be modified - * by the user. If the index is not valid, this function will throw an exception. - * - * \param index is the 0 based index of the desired parent - * - * \return constant reference to the desired link object. - */ - const COpendriveLink &get_link(unsigned int index) const; - /** - * \brief Returns a reference to the link connecting with the given road node - * - * This function returns a constant reference to the COpendriveLink object that - * connects this road node with another one, identified by its unique index. If - * there exist no connection between the two road nodes, an exception is thrown. - * - * In this case, only links starting at the road node calling this function can - * be accessed. - * - * \param node_index unique identifier of the road node candidate to check for - * connectivity. - * - * \return constant reference to the desired link object, it it exists. - */ - const COpendriveLink &get_link_ending_at(unsigned int node_index) const; - /** - * \brief Returns a reference to the link connecting with the given road node - * - * This function returns a constant reference to the COpendriveLink object that - * connects this road node with another one. If there exist no connection between - * the two road nodes, an exception is thrown. - * - * In this case, only links starting at the road node calling this function can - * be accessed. - * - * \param node COpendriveRoadNode object candidate to check for connectivity. - * - * \return constant reference to the desired link object, it it exists. - */ - const COpendriveLink &get_link_ending_at(const COpendriveRoadNode &node) const; - /** - * \brief checks if a link with another road node exists - * - * This function checks if a road node is connected through a link with another - * road node, identified by its unique index. - * - * \param node_index unique identifier of the road node candidate to check for - * connectivity. - * - * \return true if the two road nodes are connected, or false otherwise. - */ - bool has_link_with(unsigned int node_index) const; - /** - * \brief checks if a link with another road node exists - * - * This function checks if a road node is connected through a link with another - * road node. - * - * \param node COpendriveRoadNode object candidate to check for connectivity. - * - * \return true if the two road nodes are connected, or false otherwise. - */ - bool has_link_with(const COpendriveRoadNode &node) const; - /** - * \brief returns the index of the closest link to a given pose - * - * This functions finds the closest link starting on this road node to a given - * pose, and returns its index. This function only returns a valid index - * when the given pose coincides with the road node links. - * - * \param world given pose to find the closest link. - * \param distance the link distance between the projection of the given pose to - * the closest link geometry, and the position of the starting road node, - * following the closest link geometry. If the closest link does not exist - * this argument is set to the maximum double value - * (std::numeric_limits<double>::max()) - * \param only_lanes flag to indicate it the search for the closest link should include - * only the lane links (true) or all of them (false). - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the link geometry to consider each link as a candidate on the search. - * - * \return the index of the closest link or -1 if there is no link close to the given - * pose. - */ - unsigned int get_closest_link_index(TOpendriveWorldPose &pose,double &distance,bool only_lanes=false,double angle_tol=0.1) const; - /** - * \brief returns the closest link to a given pose - * - * This functions finds the closest link starting on this road node to a given - * pose, and returns it. This function only returns a valid reference when the - * given pose coincides with the road node links. Otherwise, it throws an exception. - * - * \param world given pose to find the closest link. - * \param distance the link distance between the projection of the given pose to - * the closest link geometry, and the position of the starting road node, - * following the closest link geometry. If the closest link does not exist - * this argument is set to the maximum double value - * (std::numeric_limits<double>::max()) - * \param only_lanes flag to indicate it the search for the closest link should include - * only the lane links (true) or all of them (false). - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the link geometry to consider any link as a candidate on the search. - * - * \return a constant reference to the closest link which can not be modified by the - * user. - */ - const COpendriveLink &get_closest_link(TOpendriveWorldPose &pose,double &distance,bool only_lanes=false,double angle_tol=0.1) const; - /** - * \brief returns the closest pose at any link to a given pose - * - * This functions finds the closest pose (minimum distance) on any link geometry - * to a given pose. The closest pose only exist if the imaginary line between the - * given pose and its projection into any link is orthogonal to the corresponding - * link geometry. Otherwise, it is considered that the given pose does not coincide - * with the road node links. - * - * \param world given pose to find the closest pose. - * \param closest_pose the closest pose to the closest link geometry if the given pose - * coincides with the link object. Otherwise the data on this - * parameters is not valid. - * \param only_lanes flag to indicate it the search for the closest link should include - * only the lane links (true) or all of them (false). - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the link geometry to consider any link as a candidate on the search. - * - * \return the link distance between the projection of the given pose to the - * closest link geometry, following its geometry. If there is no - * closest link, the maximum double value (std::numeric_limits<double>::max()) - * will be returned. - */ - double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes=false,double angle_tol=0.1) const; - /** - * \brief Returns a set of poses on any link close to a given pose - * - * This function is similar to the get_closest_pose(), but in this case, the closest - * pose of any link which is closer to the given pose that a given distance will be - * returned. - * - * \param world given pose to find the closest poses. - * \param closest_poses the closest poses on any link geometry if the given pose - * coincide with the link object. Otherwise an empty vector will - * be returned. - * \param distances the link distance between the projection of the given pose to each - * of the closest link geometries, following the corresponding geometry. - * If the given pose does not coincide with any of the links, an empty - * vector will be returned. - * \param dist_tol maximum allowed distance from the given position to consider any link - * as a candidate on the search. - * \param only_lanes flag to indicate it the search for the closest link should include - * only the lane links (true) or all of them (false). - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the link geometry to consider any link as a candidate on the search. - */ - void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &distances,double dist_tol,bool only_lanes=false,double angle_tol=0.1) const; - /** - * \brief overloaded stream operator - * - * This functions streams human readable information about the road node. This information - * includes: - * * The node index. - * * Its position and heading. - * * for each parent: the name of the segment and the lane id. - * * The links information. - * - */ - friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node); - /** - * \brief Destructor - */ - ~COpendriveRoadNode(); -}; - -#endif diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h deleted file mode 100644 index c9acd26a80fdbb9e7bb46ea24fcfd9a8ed3629c9..0000000000000000000000000000000000000000 --- a/include/opendrive_road_segment.h +++ /dev/null @@ -1,439 +0,0 @@ -#ifndef _OPENDRIVE_ROAD_SEGMENT_H -#define _OPENDRIVE_ROAD_SEGMENT_H - -#ifdef _HAVE_XSD -#include "xml/OpenDRIVE_1.4H.hxx" -#endif - -#include "opendrive_common.h" -#include "opendrive_lane.h" -#include "opendrive_signal.h" -#include "opendrive_object.h" -#include "opendrive_road.h" -#include "opendrive_road_node.h" -#include "opendrive_geometry.h" - -typedef struct{ - COpendriveGeometry *opendrive; - CG2Spline *spline; -}TOpendriveRoadSegmentGeometry; - -class COpendriveRoadSegment -{ - friend class COpendriveRoad; - friend class COpendriveRoadNode; - friend class COpendriveLane; - friend class COpendriveSignal; - friend class COpendriveObject; - private: - std::map<int,COpendriveLane *> lanes; - std::vector<COpendriveRoadNode *> nodes; - std::vector<TOpendriveRoadSegmentGeometry> geometries; - COpendriveRoad *parent_road; - double resolution; - double scale_factor; - double min_road_length; - int num_right_lanes; - int num_left_lanes; - std::vector<COpendriveSignal *> signals; - std::vector<COpendriveObject *> objects; - std::vector<COpendriveRoadSegment *> connecting; - std::string name; - unsigned int id; - opendrive_mark_t center_mark; - protected: - COpendriveRoadSegment(); - COpendriveRoadSegment(const COpendriveRoadSegment &object); - void load(OpenDRIVE::road_type &road_info); - void free(void); - void set_resolution(double res); - void set_scale_factor(double scale); - void set_min_road_length(double length); - void set_parent_road(COpendriveRoad *parent); - void set_name(std::string &name); - void set_id(unsigned int id); - void set_center_mark(opendrive_mark_t mark); - bool updated(segment_up_ref_t &segment_refs); - COpendriveRoadSegment *get_original_segment(segment_up_ref_t &segment_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 add_lanes(lanes::laneSection_type &lane_section); - void add_lane(const ::lane &lane_info); - void remove_lane(COpendriveLane *lane); - void add_geometries(OpenDRIVE::road_type &road_info); - void add_nodes(void); - void add_node(TOpendriveRoadSegmentGeometry &geometry,COpendriveLane *lane); - void add_empty_node(TOpendriveWorldPose &pose,COpendriveLane *lane); - void remove_node(COpendriveRoadNode *node); - bool has_node(COpendriveRoadNode *node) const; - void link_neightbour_lanes(lanes::laneSection_type &lane_section); - void link_neightbour_lanes(void); - void link_segment(COpendriveRoadSegment *segment); - 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,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL) const; - COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref) const; - TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track); - TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track); - public: - /** - * \brief Returns the Opendrive name - * - * \return The Opendrive name of the road segment - */ - std::string get_name(void) const; - /** - * \brief Returns the Opendirve numeric identifier - * - * \return The Opendrive numeric identifier of the road segment. - */ - unsigned int get_id(void) const; - /** - * \brief Returns the number of right lanes - * - * This function returns how many lanes there are on the right side of the road - * segment. The number of left and right lanes can be different. - * - * \return This function returns the number of right lanes. The range of - * valid indexes goes from -1 to the value returned by this function - * negated. - */ - unsigned int get_num_right_lanes(void) const; - /** - * \brief Returns the number of left lanes - * - * This function returns how many lanes there are on the left side of the road - * segment. The number of left and right lanes can be different. - * - * \return This function returns the number of left lanes. The range of - * valid indexes goes from 1 to the value returned by this function. - */ - unsigned int get_num_left_lanes(void) const; - /** - * \brief Returns a reference to a lane by id - * - * This function returns a constant reference to a lane of the road segment, - * which can not be modified by the user. If the identifier is not valid, this - * function will throw an exception. - * - * \param id is the Opendrive Lane identifier: negative integers values are used - * for right lanes, and positive values for left lanes. These values - * increase (for left lanes) or decrease (for right lanes) the further - * they are from the center lane of the segment. - * - * \return constant reference to a lane. - */ - const COpendriveLane &get_lane(int id) const; - /** - * \brief Returns the number of road nodes - * - * This function returns how many road nodes belong to the road segment. - * - * \return This function returns the number of road nodes. The range of - * valid indexes goes from 0 to the value returned by this function -1. - */ - unsigned int get_num_nodes(void) const; - /** - * \brief Returns a reference to a road node by local index - * - * This function returns a constant reference to the desired road node - * object, which can not be modified by the user. If the index is not valid, - * this function will throw an exception. - * - * \param index is the 0 based local index of the desired road node. It is not - * the unique index assigned to the road nodes when they are created. - * - * \return constant reference to the desired road node object. - */ - const COpendriveRoadNode &get_node(unsigned int index) const; - /** - * \brief Returns a reference to the parent road - * - * This function returns a constant reference to the parent road object, which - * can not be modified by the user. If the internal parent road is invalid, this - * function will throw an exception. - * - * \return a constant reference to the parent road object. - */ - const COpendriveRoad &get_parent_road(void) const; - /** - * \brief Returns the number of road signals - * - * This function returns how many road signals are placed at this road segment. - * - * \return This function returns the number of road signals. The range of - * valid indexes goes from 0 to the value returned by this function -1. - */ - unsigned int get_num_signals(void) const; - /** - * \brief Returns a reference to a road signal by local index - * - * This function returns a constant reference to the desired road signal - * object, which can not be modified by the user. If the index is not valid, - * this function will throw an exception. - * - * \param index is the 0 based local index of the desired road signal. - * - * \return constant reference to the desired road signal object. - */ - const COpendriveSignal &get_signal(unsigned int index) const; - /** - * \brief Returns the number of road objects - * - * This function returns how many road objects are placed at this road segment. - * - * \return This function returns the number of road objects. The range of - * valid indexes goes from 0 to the value returned by this function -1. - */ - unsigned int get_num_objects(void) const; - /** - * \brief Returns a reference to a road object by local index - * - * This function returns a constant reference to the desired road object - * object, which can not be modified by the user. If the index is not valid, - * this function will throw an exception. - * - * \param index is the 0 based local index of the desired road object. - * - * \return constant reference to the desired road object object. - */ - const COpendriveObject &get_object(unsigned int index) const; - /** - * \brief Returns the number of connecting road segments - * - * This function returns how many road segments are connected to this road - * segment. Since the road segments have to possible traffic directions, the - * concept of next and previous is only defined when the side of the road is - * selected. - * - * \return This function returns the number of connecting road segments. The - * range of valid indexes goes from 0 to the value returned by this - * function -1. - */ - unsigned int get_num_connecting(void) const; - /** - * \brief Returns a reference to a connecting road segment by local index - * - * This function returns a constant reference to the desired connecting road - * segment object, which can not be modified by the user. If the index is - * not valid, this function will throw an exception. - * - * \param index is the 0 based local index of the desired connecting road - * segment. - * - * \return constant reference to the desired connecting road segment object. - */ - const COpendriveRoadSegment &get_connecting(unsigned int index) const; - /** - * \brief returns the length of the road segment - * - * This function returns the length of the road segment geometry. The road - * segment trajectory is represented by Opendive geometry objects and also - * by G2 Splines, and it is different from the link and lanes geometries. - * Both representations are equivalent. - * - * \return length of the road segment in meters. - */ - double get_length(void) const; - /** - * \brief returns the pose of the road segment at a given point - * - * This function returns the pose (x and y coordinates and heading) of the center - * lane geometry of the road segment at a given distance from the origin point - * of the road segment. - * - * The road segment trajectory is represented by Opendive geometry objects and also - * by G2 Splines, and it is different from the link and lanes geometries. - * Both representations are equivalent. - * - * \param length desired distance from the origin point of the road segment in meters. - * This distance must be smaller or equal than the actual road segment - * length. - * - * \return the pose at the desired point in the world reference system. - */ - TOpendriveWorldPose get_pose_at(double length) const; - /** - * \brief returns the curvature of the road segment at a given point - * - * This function returns the curvature of the center geometry of the road segment - * at a given distance from the origin point of the road segment. - * - * The road segment trajectory is represented by Opendive geometry objects and also - * by G2 Splines, and it is different from the link and lanes geometries. - * Both representations are equivalent. - * - * \param length desired distance from the origin point of the road segment in meters. - * This distance must be smaller or equal than the actual road segment - * length. - * - * \return the curvature at the desired point. - */ - double get_curvature_at(double length) const; - /** - * \brief returns the closest node to a given pose - * - * This functions finds the closest road node belonging to the road segment to a - * given pose, and returns it. This function only returns a valid reference when the - * given pose coincides with the road segment. Otherwise, it throws an exception. - * - * \param world given pose to find the closest road node. - * \param distance the node distance between the projection of the given pose to - * the lane link geometry of the closest road node, and the start - * position of the node, following the corresponding lane link geometry. - * If the closest road node does not exist this argument is set to the - * maximum double value (std::numeric_limits<double>::max()) - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane link geometry of the closest road node. - * - * \return a constant reference to the closest node which can not be modified by the - * user. - */ - const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the index of the closest node to a given pose - * - * This functions finds the closest road node belonging to the road segment to a given - * pose, and returns its index. This function only returns a valid index - * when the given pose coincides with the road segment. - * - * \param world given pose to find the closest road node. - * \param distance the node distance between the projection of the given pose to - * the lane link geometry of the closest road node, and the start - * position of the node, following the corresponding lane link geometry. - * If the closest road node does not exist this argument is set to the - * maximum double value (std::numeric_limits<double>::max()) - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane link geometry of the closest road node. - * - * \return the index of the closest node or -1 if there is no node close to the given - * pose. - */ - unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the closest lane to a given pose - * - * This functions finds the closest lane belonging to the road segment to a - * given pose, and returns it. This function only returns a valid reference when the - * given pose coincides with the road segment. Otherwise, it throws an exception. - * - * \param world given pose to find the closest lane. - * \param distance the lane distance between the projection of the given pose to - * the lane geometry, and the start position of the lane, following - * its geometry. If the closest lane does not exist this argument - * is set to the maximum double value (std::numeric_limits<double>::max()) - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane geometry. - * - * \return a constant reference to the closest lane which can not be modified by the - * user. - */ - const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the id of the closest lane to a given pose - * - * This functions finds the closest lane belonging to the road segment to a given - * pose, and returns its id. This function only returns a valid id - * when the given pose coincides with the road segment. - * - * \param world given pose to find the closest lane. - * \param distance the lane distance between the projection of the given pose to - * the lane geometry, and the start position of the lane, following - * its geometry. If the closest lane does not exist this argument - * is set to the maximum double value (std::numeric_limits<double>::max()) - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the lane geometry. - * - * \return the id of the closest node or 0 if there is no lane close to the given - * pose. - */ - int get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const; - /** - * \brief returns the closest pose to a given pose - * - * This functions finds the closest pose (minimum distance) on the road segment geometry - * to a given pose. The closest pose only exist if the imaginary line between the - * given pose and its projection into the road segment is orthogonal to the segment geometry. - * Otherwise, it is considered that the given pose does not coincide with the road segment. - * - * \param world given pose to find the closest pose. - * \param closest_pose the closest pose to the center lane of the road segment if the - * given pose coincides with it. Otherwise the data on this - * parameters is not valid. - * \param angle_tol maximum heading difference between the given pose and the closest pose - * on the road segment geometry. - * - * \return the road segment distance between the projection of the given pose to the - * center lane geometry of the road segment, following its geometry. If the - * closest pose does not exist, the maximum double value - * (std::numeric_limits<double>::max()) will be returned. - */ - double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const; - /** - * \brief returns a reference to the next road segments. - * - * This function returns a constant reference to the set of road segments that will follow - * this road segment, depending on the road side of interest. - * - * \param input_side an integer indicating the road side of interest of the current - * road segment. It is negative for the right side and positive for - * the left one (similar to the lane id criteria). - * \param output_sides output parameter that indicates the side of the next road segments - * that connects with the side of interest of the current one. It - * is negative for the right side and positive for the left one. If - * there are no next road segments, this vector will be empty. - * - * \return a vector with the constant references of the next road segments. If there - * are no next road segments, this vector will be empty. - */ - std::vector<const COpendriveRoadSegment *> get_next_segments(int input_side,std::vector<int> &output_sides) const; - /** - * \brief returns a reference to the previous road segments. - * - * This function returns a constant reference to the set of road segments that preceed - * this road segment, depending on the road side of interest. - * - * \param input_side an integer indicating the road side of interest of the current - * road segment. It is negative for the right side and positive for - * the left one (similar to the lane id criteria). - * \param output_sides output parameter that indicates the side of the previous road segments - * that connects with the side of interest of the current one. It - * is negative for the right side and positive for the left one. If - * there are no previous road segments, this vector will be empty. - * - * \return a vector with the constant references of the previous road segments. If there - * are no previous road segments, this vector will be empty. - */ - std::vector<const COpendriveRoadSegment *> get_prev_segments(int input_side,std::vector<int> &output_sides) const; - /** - * \brief Returns the road segment side of a given pose - * - * This function returns the road segment side of a given pose. If the given pose - * does not coincide with the road segment, an exception will be thrown. - * - * \return an integer indicating the road side of the given pose. It is negative for - * the right side and positive for the left one (similar to the lane id criteria). - */ - int get_pose_side(TOpendriveWorldPose &pose) const; - /** - * \brief overloaded stream operator - * - * This functions streams human readable information about the road segment. This information - * includes: - * * The name of the connecting road segments. - * * The road nodes information. - * * The left lanes information. - * * The right lanes information. - * * The road signals information. - * * The road objects information. - * - */ - friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment); - /** - * \brief Destructor - */ - ~COpendriveRoadSegment(); -}; - -#endif diff --git a/include/osm/osm_common.h b/include/osm/osm_common.h new file mode 100644 index 0000000000000000000000000000000000000000..843ffcc27e1c43f141539c0016cecb165f24d800 --- /dev/null +++ b/include/osm/osm_common.h @@ -0,0 +1,15 @@ +#ifndef _OSM_COMMON_H +#define _OSM_COMMON_H + +#define DEFAULT_LANE_WIDTH 4.0 +#define DEFAULT_MIN_RADIUS 5.0 +#define MIN_ROAD_LENGTH 0.5 + +class COSMMap; +class COSMNode; +class COSMWay; +class COSMJunction; +class COSMRoadSegment; +class COSMRestriction; + +#endif diff --git a/include/osm/osm_junction.h b/include/osm/osm_junction.h new file mode 100644 index 0000000000000000000000000000000000000000..3db8e36ad77ade8400ff927cca3366903d566b73 --- /dev/null +++ b/include/osm/osm_junction.h @@ -0,0 +1,50 @@ +#ifndef _OSM_JUNCTION_H +#define _OSM_JUNCTION_H + +#include "osm/osm_common.h" +#include "osm/osm_node.h" +#include "osm/osm_road_segment.h" + +#include <Eigen/Dense> +#include <vector> + +#include "junction.h" + +typedef struct{ + COSMWay *in_way; + int in_lane_id; + COSMWay *out_way; + int out_lane_id; +}TOSMLink; + +class COSMJunction +{ + friend class COSMMap; + friend class COSMNode; + friend class COSMWay; + friend class COSMRoadSegment; + private: + std::vector<COSMRoadSegment *> in_roads; + std::vector<COSMRoadSegment *> out_roads; + COSMNode *parent_node; + std::vector<std::vector<Eigen::MatrixXi>> connections; + std::vector<TOSMLink> links; + protected: + void create_FF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix); + void create_FB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix); + void create_BF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix); + void create_BB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lines,Eigen::MatrixXi &matrix); + void create_initial_connectivity(void); + void apply_node_restrictions(void); + void apply_way_restrictions(void); + void create_links(void); + bool add_link(TOSMLink &new_link); + void convert(CJunction **junction,osm_road_map_t &road_map); + public: + COSMJunction(COSMNode *node); + COSMNode &get_parent_node(void); + friend std::ostream& operator<<(std::ostream& out, COSMJunction &junction); + ~COSMJunction(); +}; + +#endif diff --git a/include/osm/osm_map.h b/include/osm/osm_map.h new file mode 100644 index 0000000000000000000000000000000000000000..1bf2f79e51230b43ec870a35a97887dd7592747e --- /dev/null +++ b/include/osm/osm_map.h @@ -0,0 +1,56 @@ +#ifndef _OSM_MAP_H +#define _OSM_MAP_H + +#include "osm/osm_common.h" +#include "osm/osm_way.h" +#include "osm/osm_node.h" +#include "osm/osm_restriction.h" + +#include "road_map.h" + +#include <osmium/handler.hpp> +#include <osmium/io/header.hpp> +#include <osmium/tags/tags_filter.hpp> + +class COSMMap : public osmium::handler::Handler +{ + friend class COSMWay; + friend class COSMNode; + friend class COSMRestriction; + private: + std::vector<COSMRoadSegment *> segments; + std::vector<COSMJunction *> junctions; + std::vector<COSMNode *> nodes; + std::vector<COSMWay *> ways; + std::vector<COSMRestriction *> restrictions; + osmium::TagsFilter way_filter; + osmium::TagsFilter relation_filter; + long int max_id; + int utm_zone; + protected: + void free(void); + void process_map(void); + void create_junctions(void); + void create_road_segments(void); + void delete_way(COSMWay *way); + void add_way(COSMWay *way); + COSMWay *get_way_by_id(long int id); + void delete_node(COSMNode *node); + void add_node(COSMNode *node); + COSMNode *get_node_by_id(long int id); + void add_restriction(COSMRestriction *restriction); + void normalize_locations(std::vector<osmium::Box> &boxes); + public: + COSMMap(); + void set_utm_zone(int zone); + int get_utm_zone(void); + void load(const std::string &filename); + void node(const osmium::Node& node); + void way(const osmium::Way& way); + void relation(const osmium::Relation& relation); + void convert(CRoadMap &road); + friend std::ostream& operator<<(std::ostream& out, COSMMap &map); + ~COSMMap(); +}; + +#endif diff --git a/include/osm/osm_node.h b/include/osm/osm_node.h new file mode 100644 index 0000000000000000000000000000000000000000..532b13b5dd36e2e8de14431fe869b0ab59f541cd --- /dev/null +++ b/include/osm/osm_node.h @@ -0,0 +1,64 @@ +#ifndef _OSM_NODE_H +#define _OSM_NODE_H + +#include "osm/osm_common.h" +#include "osm/osm_map.h" +#include "osm/osm_way.h" +#include "osm/osm_junction.h" +#include "osm/osm_restriction.h" + +#include <vector> +#include <map> + +#include <osmium/osm/node.hpp> + +typedef struct +{ + double x; + double y; +}TLocation; + +class COSMNode +{ + friend class COSMMap; + friend class COSMWay; + friend class COSMJunction; + friend class COSMRestriction; + private: + long int id; + std::vector<COSMWay *> ways; + TLocation location; + COSMJunction *junction; + std::vector<COSMRestriction *> restrictions; + COSMMap *parent; + protected: + void set_parent(COSMMap *aprent); + void add_way(COSMWay *new_way); + void delete_way(COSMWay *way); + void update_way(COSMWay *old_way,COSMWay *new_way); + bool merge_ways(void); + void split_ways(void); + void remove_in_junction_nodes(void); + void add_restriction(COSMRestriction *new_restriction); + void delete_restriction(COSMRestriction *restriction); + void clear_restrictions(void); + void normalize_location(double center_x,double center_y); + COSMNode(const osmium::Node &node,COSMMap *parent); + public: + COSMNode(const COSMNode &object); + long int get_id(void); + unsigned int get_num_ways(void); + COSMWay &get_way_by_index(unsigned int index); + COSMWay &get_way_by_id(long int id); + void get_location(double &x, double &y); + double compute_distance(COSMNode &node); + double compute_heading(COSMNode &node); + double compute_angle(COSMNode &node1,COSMNode &node2); + unsigned int get_num_restrictions(void); + COSMRestriction &get_restriction(unsigned int index); + COSMMap &get_parent(void); + friend std::ostream& operator<<(std::ostream& out,COSMNode &node); + ~COSMNode(); +}; + +#endif diff --git a/include/osm/osm_restriction.h b/include/osm/osm_restriction.h new file mode 100644 index 0000000000000000000000000000000000000000..88e5a96fac0e5fc42dadd3738d88f591452449c7 --- /dev/null +++ b/include/osm/osm_restriction.h @@ -0,0 +1,45 @@ +#ifndef _OSM_RESTRICTION_H +#define _OSM_RESTRICTION_H + +#include "osm/osm_map.h" +#include "osm/osm_node.h" +#include "osm/osm_way.h" + +#include <osmium/osm/relation.hpp> + +typedef enum {RESTRICTION_NONE,RESTRICTION_NO,RESTRICTION_ONLY} restriction_action_t; +typedef enum {RESTRICTION_LEFT_TURN,RESTRICTION_RIGHT_TURN,RESTRICTION_U_TURN,RESTRICTION_STRAIGHT_ON} restriction_type_t; + +class COSMRestriction +{ + friend class COSMMap; + friend class COSMNode; + friend class COSMWay; + private: + COSMMap *parent; + long int id; + COSMWay *from; + COSMWay *to; + COSMNode *via; + restriction_action_t action; + restriction_type_t type; + protected: + void set_parent(COSMMap *parent); + void update_to_way(COSMWay *old_way,COSMWay *new_way); + void update_from_way(COSMWay *old_way,COSMWay *new_way); + COSMRestriction(COSMWay *from,COSMNode *via,COSMWay *to,restriction_action_t action, restriction_type_t type,COSMMap *parent); + COSMRestriction(const osmium::Relation &relation,COSMMap *parent); + COSMRestriction(const COSMRestriction &object); + public: + COSMRestriction(); + long int get_id(void); + COSMWay &get_from_way(void); + COSMWay &get_to_way(void); + COSMNode &get_via_node(void); + restriction_action_t get_action(void); + restriction_type_t get_type(void); + COSMMap &get_parent(void); + ~COSMRestriction(); +}; + +#endif diff --git a/include/osm/osm_road_segment.h b/include/osm/osm_road_segment.h new file mode 100644 index 0000000000000000000000000000000000000000..14b0205e71b71557eaf214432a05c54717a19568 --- /dev/null +++ b/include/osm/osm_road_segment.h @@ -0,0 +1,42 @@ +#ifndef _OSM_ROAD_SEGMENT_H +#define _OSM_ROAD_SEGMENT_H + +#include "osm/osm_common.h" +#include "osm/osm_way.h" +#include "osm/osm_junction.h" + +#include "road.h" + +#include <iostream> + + +class COSMRoadSegment +{ + friend class COSMJunction; + friend class COSMMap; + friend class COSMWay; + friend class CRoad; + private: + COSMWay *parent_way; + COSMJunction *start_junction; + COSMJunction *end_junction; + double start_distance; + double original_start_distance; + double end_distance; + double original_end_distance; + protected: + void set_start_distance(double dist); + void set_end_distance(double dist); + bool generate_fwd_geometry(CRoad *road,double resolution); + bool generate_bwd_geometry(CRoad *road,double resolution); + void convert(CRoad **left_road,CRoad **right_road,double resolution); + public: + COSMRoadSegment(COSMWay *way); + COSMWay &get_parent_way(void); + double get_start_distance(void); + double get_end_distance(void); + friend std::ostream& operator<<(std::ostream& out, COSMRoadSegment &segment); + ~COSMRoadSegment(); +}; + +#endif diff --git a/include/osm/osm_way.h b/include/osm/osm_way.h new file mode 100644 index 0000000000000000000000000000000000000000..3e3bf98904ff9531532a7e7246bac81e1b159c2b --- /dev/null +++ b/include/osm/osm_way.h @@ -0,0 +1,90 @@ +#ifndef _OSM_WAY_H +#define _OSM_WAY_H + +#include "osm/osm_common.h" +#include "osm/osm_map.h" +#include "osm/osm_node.h" +#include "osm/osm_road_segment.h" +#include "osm/osm_restriction.h" + +#include <map> +#include <vector> + +#include <osmium/osm/way.hpp> + +#define LAST_SEGMENT -1 +#define FIRST_SEGMENT 0 + +typedef enum {NO_RESTRICTION=0x00,RESTRICTION_THROUGH=0x01,RESTRICTION_LEFT=0x02,RESTRICTION_RIGHT=0x04} lane_restructions_t; + +class COSMWay +{ + friend class COSMMap; + friend class COSMNode; + friend class COSMJunction; + friend class COSMRoadSegment; + friend class COSMRestriction; + private: + long int id; + std::string name; + std::vector<COSMNode *> nodes; + unsigned int num_forward_lanes; + std::vector<lane_restructions_t> forward_lanes; + unsigned int num_backward_lanes; + std::vector<lane_restructions_t> backward_lanes; + double lane_width; + COSMRoadSegment *road_segment; + COSMMap *parent; + std::vector<COSMRestriction *> restrictions; + protected: + void set_parent(COSMMap *parent); + void add_node(COSMNode *new_node); + void delete_node(COSMNode *node); + void update_node(COSMNode *old_node,COSMNode *new_node); + bool is_node_first(long int id); + bool is_node_last(long int id); + bool is_node_left(COSMNode &node,bool forward=true,double tol=0.5); + bool is_node_right(COSMNode &node,bool forward=true,double tol=0.5); + void remove_straight_nodes(void); + void add_restriction(COSMRestriction *new_restriction); + void delete_restriction(COSMRestriction *restriction); + void delete_restriction(long int id); + void update_restriction(COSMWay *old_way,COSMWay *new_way); + void clear_restrictions(void); + COSMWay *split(long int node_id); + void merge(COSMWay *way); + COSMWay(const osmium::Way &way,COSMMap *parent); + bool is_not_connected(void); + bool load_turn_lane_tag(const osmium::Way &way,const std::string &tag,std::vector<lane_restructions_t> &restrictions,unsigned int max_lanes); + void load_lane_restrictions(const osmium::Way &way); + public: + COSMWay(const COSMWay &object); + long int get_id(void); + std::string get_name(void); + unsigned int get_num_nodes(void); + COSMNode &get_node_by_index(unsigned int index); + COSMNode &get_next_node_by_index(unsigned int index); + COSMNode &get_prev_node_by_index(unsigned int index); + COSMNode &get_node_by_id(long int id); + COSMNode &get_next_node_by_id(long int id); + COSMNode &get_prev_node_by_id(long int id); + COSMNode &get_closest_node(COSMNode &node); + unsigned int get_node_multiplicity(long int id); + bool has_node(long int id); + unsigned int get_num_segments(void); + COSMNode &get_segment_start_node(unsigned int index); + COSMNode &get_segment_end_node(unsigned int index); + unsigned int get_closest_segment(COSMNode &node); + unsigned int get_num_lanes(void); + unsigned int get_num_forward_lanes(void); + unsigned int get_num_backward_lanes(void); + bool is_one_way(void); + lane_restructions_t get_lane_restriction(unsigned int index); + double get_lane_width(void); + COSMRoadSegment &get_road_segment(void); + COSMMap &get_parent(void); + friend std::ostream& operator<<(std::ostream& out, COSMWay &way); + ~COSMWay(); +}; + +#endif diff --git a/include/road.h b/include/road.h new file mode 100644 index 0000000000000000000000000000000000000000..3af83a6a1a6df723a7744e8b117c3ee22c00a878 --- /dev/null +++ b/include/road.h @@ -0,0 +1,81 @@ +#ifndef _ROAD_H +#define _ROAD_H + +#include "road_map.h" +#include "junction.h" +#include "road_segment.h" +#include "opendrive/opendrive_road_segment.h" + +class CRoad +{ + friend class CJunction; + friend class CRoadMap; + friend class CRoadSegment; + friend class COpendriveRoadSegment; + friend class COSMRoadSegment; + private: + unsigned int id; + double resolution; + /* connectivity */ + CRoadMap *parent_roadmap; + CJunction *prev_junction; + CJunction *next_junction; + CRoad *opposite_direction_road; + /* geometry */ + std::vector<CRoadSegment *> segments; + TPoint start_point; + unsigned int num_lanes; + double lane_width; + double lane_speed; + protected: + CRoad(unsigned int id); + static unsigned int get_index_by_id(const std::vector<CRoad *> &roads,unsigned int id); + void set_id(unsigned int id); + /* connectivity */ + void set_parent_roadmap(CRoadMap *roadmap); + void set_prev_junction(CJunction *junction); + void set_next_junction(CJunction *junction); + void set_opposite_direction_road(CRoad *road); + /* geometry */ + CRoadSegment *get_first_segment(void); + CRoadSegment *get_last_segment(void); + CRoadSegment *get_segment_by_index(unsigned int index); + void merge(CRoad *road); + public: + CRoad(); + void set_resolution(double resolution); + double get_resolution(void); + CRoadMap &get_parent_roadmap(void); + unsigned int get_id(void); + /* connectivity */ + bool exist_prev_junction(void); + CJunction &get_prev_junction(void); + bool exist_next_junction(void); + CJunction &get_next_junction(void); + bool has_opposite_direction_road(void); + CRoad &get_opposite_direction_road(void); + /* geometry */ + void set_num_lanes(unsigned int num); + unsigned int get_num_lanes(void); + void set_lane_width(double width); + double get_lane_width(void); + void set_lane_speed(double speed); + double get_lane_speed(void); + unsigned int get_num_segments(void); + CRoadSegment &get_segment_by_id(unsigned int id); + void set_start_point(TPoint &start_point); + void set_start_point(double x,double y,double heading, double curvature); + void add_segment(TPoint &end_point); + void add_segment(double x,double y,double heading, double curvature); + void remove_segment_by_index(unsigned int index); + bool get_point_at(double distance, double lateral_offset,TPoint &point); + bool get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max()); + + bool get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max()); + bool get_segment_id_at(double distance,unsigned int &segment_id); + void get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw); + void get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw); + ~CRoad(); +}; + +#endif diff --git a/include/road_map.h b/include/road_map.h new file mode 100644 index 0000000000000000000000000000000000000000..f7d8138deb7049858a20aef69ffaa164ea611537 --- /dev/null +++ b/include/road_map.h @@ -0,0 +1,87 @@ +#ifndef _ROAD_MAP_H +#define _ROAD_MAP_H + +#include "road.h" +#include "junction.h" +#include "road_segment.h" +#include "common.h" + +#include "opendrive/opendrive_road_segment.h" +#include "opendrive/opendrive_junction.h" + +#include <Eigen/Dense> +#include <vector> + +class CRoad; +class CJunction; +class CRoadSegment; + +typedef struct{ + unsigned int segment_id; + unsigned int lane_id; +}TLaneID; + +class CRoadMap +{ + friend class CRoad; + friend class CJunction; + friend class CRoadSegment; + friend class COpendriveRoadSegment; + friend class COpendriveJunction; + private: + unsigned int next_segment_id; + unsigned int next_road_id; + unsigned int next_junction_id; + /* connectivity */ + std::vector<CRoad *> roads; + std::vector<CJunction *> junctions; + double resolution; + protected: + void free(void); + unsigned get_next_segment_id(void); + unsigned get_next_road_id(void); + unsigned get_next_junction_id(void); + CRoad *get_road_by_index(unsigned int index); + CJunction *get_junction_by_index(unsigned int index); + void get_segment_by_id(unsigned int id,CRoadSegment **segment); + void remove_road_by_index(unsigned int index); + void set_opposite_direction_roads_by_index(unsigned int right_road,unsigned int left_road); + void remove_junction_by_index(unsigned int index); + void merge_roads(opendrive_road_map_t &road_map); + public: + CRoadMap(); + void load_opendrive(const std::string &filename); + void save_opendrive(const std::string &filename); + void load_osm(const std::string &filename); + void set_resolution(double resolution); + double get_resolution(void); + std::vector<unsigned int> get_path_sub_roadmap(std::vector<unsigned int> &segment_ids,CRoadMap &new_road_map); + double get_path_length(std::vector<unsigned int> &segment_ids); + /* connectivity */ + unsigned int get_num_roads(void); + CRoad &get_road_by_id(unsigned int id); + unsigned int get_num_junctions(void); + CJunction &get_junction_by_id(unsigned int id); + unsigned int get_num_segments(void); + CRoadSegment &get_segment_by_id(unsigned int id); + void add_road(CRoad *road); + bool has_road(unsigned int id); + void remove_road_by_id(unsigned int id); + void set_opposite_direction_roads_by_id(unsigned int right_road,unsigned int left_road); + void add_junction(CJunction *junction); + bool has_junction(unsigned int id); + void remove_junction_by_id(unsigned int id); + void get_road_connectivity(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &id_map); + void get_segment_connectivity(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &id_map); + void get_lane_connectivity(Eigen::MatrixXi &connectivity,std::vector<TLaneID> &id_map); + /* geometry */ + unsigned int get_max_num_lanes(void); + bool get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max()); + bool get_closest_segment_ids(TPoint &point,std::vector<unsigned int >&segment_ids,std::vector<double >&distances,std::vector<double >&lateral_offsets,double angle_tol=std::numeric_limits<double>::max(),double offset_tol=std::numeric_limits<double>::max()); + void get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw); + void get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw); + + ~CRoadMap(); +}; + +#endif diff --git a/include/road_segment.h b/include/road_segment.h new file mode 100644 index 0000000000000000000000000000000000000000..72310e13aed80b3d469d89de93e2fa64109dbf41 --- /dev/null +++ b/include/road_segment.h @@ -0,0 +1,74 @@ +#ifndef _ROAD_SEGMENT_H +#define _ROAD_SEGMENT_H + +#include "g2_spline.h" +#include "road.h" +#include "junction.h" + +#include <Eigen/Dense> + +class CRoad; +class CJunction; + +class CRoadSegment +{ + friend class CRoad; + friend class CJunction; + friend class CRoadMap; + private: + unsigned int id; + double resolution; + CRoad *parent_road; + CJunction *parent_junction; + std::vector<CRoadSegment *> prev_segments; + std::vector<CRoadSegment *> next_segments; + Eigen::MatrixXd connectivity; + TPoint start_point; + TPoint end_point; + CG2Spline spline; + protected: + static unsigned int get_index_by_id(const std::vector<CRoadSegment *> &segments,unsigned int id); + void set_id(unsigned int id); + void set_parent_road(CRoad *road); + void set_parent_junction(CJunction *junction); + void add_prev_segment(CRoadSegment *segment); + bool has_prev_segment(CRoadSegment *segment); + void remove_prev_segment(CRoadSegment *segment); + void add_next_segment(CRoadSegment *segment); + bool has_next_segment(CRoadSegment *segment); + void remove_next_segment(CRoadSegment *segment); + public: + CRoadSegment(unsigned int lanes_in,unsigned int lanes_out); + unsigned int get_id(void); + void set_resolution(double resolution); + double get_resolution(void); + bool has_parent_road(void); + CRoad &get_parent_road(void); + bool has_parent_junction(void); + CJunction &get_parent_junction(void); + unsigned int get_num_prev_segments(void); + CRoadSegment &get_prev_segment_by_index(unsigned int index); + unsigned int get_num_next_segments(void); + CRoadSegment &get_next_segment_by_index(unsigned int index); + unsigned int get_num_lanes_in(void); + unsigned int get_num_lanes_out(void); + double get_lane_width(void); + double get_lane_speed(void); + unsigned int get_lane(double lateral_offset); + void set_geometry(TPoint &start_point,TPoint &end_point); + void get_start_point(TPoint &point); + void get_end_point(TPoint &point); + void set_full_connectivity(void); + void link_lanes(unsigned int lane1,unsigned int lane2); + void unlink_lanes(unsigned int lane1,unsigned int lane2); + bool are_lanes_linked(unsigned int lane1,unsigned int lane2); + void get_connectivity_matrix(Eigen::MatrixXd &connectivity); + bool is_input_lane_connected(unsigned int lane); + double get_length(void); + bool get_point_at(double distance, double lateral_offset,TPoint &point); + bool get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max()); + void get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double lateral_offset=0.0,double start_length=0.0, double end_length=std::numeric_limits<double>::max()); + ~CRoadSegment(); +}; + +#endif diff --git a/include/vel_profile.h b/include/vel_profile.h new file mode 100644 index 0000000000000000000000000000000000000000..abde16e31cfe541b12a3fbb84130a8a8c16322df --- /dev/null +++ b/include/vel_profile.h @@ -0,0 +1,44 @@ +#ifndef _VEL_PROFILE_H +#define _VEL_PROFILE_H + +#include <vector> +#include <ostream> + +class CVelProfile +{ + protected: + double start_vel; + double end_vel; + double max_vel; + double start_acc; + double end_acc; + double max_acc; + double time; + double length; + bool generated; + public: + CVelProfile(); + CVelProfile(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc); + void set_start_vel(double vel); + double get_start_vel(void); + void set_end_vel(double vel); + double get_end_vel(void); + void set_max_vel(double vel); + double get_max_vel(void); + void set_start_acc(double acc); + double get_start_acc(void); + void set_end_acc(double acc); + double get_end_acc(void); + void set_max_acc(double acc); + double get_max_acc(void); + double get_time(void); + double get_length(void); + // evaluation functions + virtual void evaluate_at_time(double time,double &vel, double &acc,double &len,double start_len=0.0)=0; + virtual void evaluate_at_length(double length,double &vel, double &acc,double &time)=0; + virtual void evaluate_all(double resolution,std::vector<double> &vel,std::vector<double> &acc,std::vector<double> &len,double start_len=0.0)=0; + friend std::ostream& operator<< (std::ostream& out, const CVelProfile &profile); + virtual ~CVelProfile(); +}; + +#endif diff --git a/include/vel_spline.h b/include/vel_spline.h new file mode 100644 index 0000000000000000000000000000000000000000..983c4f563d6ca481871c30682cf49e8b3aeecac6 --- /dev/null +++ b/include/vel_spline.h @@ -0,0 +1,38 @@ +#ifndef _VEL_SPLINE_H +#define _VEL_SPLINE_H + +#include "vel_profile.h" +#include "gradient.h" + +#define DEFAULT_MAX_ACC 1.5 + +class CVelSpline : public CVelProfile +{ + private: + double coeff[4]; + // optimization attributes + CGradient time_grad; + double target_length; + protected: + double length_pow2(double time); + double length_pow2_der(double time); + void set_target_length(double length); + void compute_max_vel(void); + public: + CVelSpline(); + CVelSpline(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc); + // generation functions + void generate_max_acc(double max_acc); + bool generate_max_length(double &max_len); + void generate_max_acc(double max_acc,double start_vel,double start_acc,double end_vel,double end_acc=0.0); + bool generate_max_length(double &max_len,double start_vel,double start_acc,double end_vel,double end_acc=0.0); + void generate_constant_vel(double vel, double length); + void update_max_start_vel(double max_acc,double length,double max_end_vel); + // evaluation functions + void evaluate_at_time(double time,double &vel, double &acc,double &len,double start_len=0.0); + void evaluate_at_length(double length,double &vel, double &acc,double &time); + void evaluate_all(double resolution,std::vector<double> &vel,std::vector<double> &acc,std::vector<double> &len,double start_len=0.0); + ~CVelSpline(); +}; + +#endif diff --git a/include/vel_trapezoid.h b/include/vel_trapezoid.h new file mode 100644 index 0000000000000000000000000000000000000000..6a477ab89da345c85981164a61005ea9675d6b56 --- /dev/null +++ b/include/vel_trapezoid.h @@ -0,0 +1,25 @@ +#ifndef _VEL_TRAPEZOID_H +#define _VEL_TRAPEZOID_H + +#include "vel_profile.h" + +class CVelTrapezoid : public CVelProfile +{ + private: + double acc_time; + double acc_length; + double dec_time; + double dec_length; + protected: + void generate(double start_vel,double end_vel,double max_vel, double max_acc,double length); + public: + CVelTrapezoid(); + CVelTrapezoid(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc); + bool generate(double start_vel,double end_vel,double max_vel, double max_acc,double length,double &new_start_vel,double &new_max_vel); + void evaluate_at_time(double time,double &vel, double &acc,double &len,double start_len=0.0); + void evaluate_at_length(double length,double &vel, double &acc,double &time); + void evaluate_all(double resolution,std::vector<double> &vel,std::vector<double> &acc,std::vector<double> &len,double start_len=0.0); + ~CVelTrapezoid(); +}; + +#endif diff --git a/libosmium/FindGem.cmake b/libosmium/FindGem.cmake new file mode 100644 index 0000000000000000000000000000000000000000..9927ea81438c15d247c1ed07b29d1dfb6feb9ccf --- /dev/null +++ b/libosmium/FindGem.cmake @@ -0,0 +1,155 @@ +# Author thomas.roehr@dfki.de +# +# Version 0.31 2017-09-15 +# - find gem executable gem.cmd on Windows +# Version 0.3 2013-07-02 +# - rely on `gem content` to find library and header +# - introduce GEM_OS_PKG to allow search via pkgconfig +# Version 0.2 2010-01-14 +# - add support for searching for multiple gems +# Version 0.1 2010-12-15 +# - support basic search functionality +# - tested to find rice +# +# OUTPUT: +# +# GEM_INCLUDE_DIRS After successful search contains the include directores +# +# GEM_LIBRARIES After successful search contains the full path of each found library +# +# +# Usage: +# set(GEM_DEBUG TRUE) +# find_package(Gem COMPONENTS rice hoe) +# include_directories(${GEM_INCLUDE_DIRS}) +# target_link_libraries(${GEM_LIBRARIES} +# +# in case pkg-config should be used to search for the os pkg, set GEM_OS_PKG, i.e. +# set(GEM_OS_PKG TRUE) +# +# Check for how 'gem' should be called +include(FindPackageHandleStandardArgs) +find_program(GEM_EXECUTABLE + NAMES "gem.cmd" "gem${RUBY_VERSION_MAJOR}${RUBY_VERSION_MINOR}" + "gem${RUBY_VERSION_MAJOR}.${RUBY_VERSION_MINOR}" + "gem-${RUBY_VERSION_MAJOR}${RUBY_VERSION_MINOR}" + "gem-${RUBY_VERSION_MAJOR}.${RUBY_VERSION_MINOR}" + "gem${RUBY_VERSION_MAJOR}${RUBY_VERSION_MINOR}${RUBY_VERSION_PATCH}" + "gem${RUBY_VERSION_MAJOR}.${RUBY_VERSION_MINOR}.${RUBY_VERSION_PATCH}" + "gem-${RUBY_VERSION_MAJOR}${RUBY_VERSION_MINOR}${RUBY_VERSION_PATCH}" + "gem-${RUBY_VERSION_MAJOR}.${RUBY_VERSION_MINOR}.${RUBY_VERSION_PATCH}" + "gem") + +# Making backward compatible +if(Gem_DEBUG) + set(GEM_DEBUG TRUE) +endif() + +if(NOT GEM_EXECUTABLE) + MESSAGE(FATAL_ERROR "Could not find the gem executable - install 'gem' first") +endif() + +if(NOT Gem_FIND_COMPONENTS) + MESSAGE(FATAL_ERROR "If searching for a Gem you have to provide COMPONENTS with the name of the gem") +endif() + +foreach(Gem_NAME ${Gem_FIND_COMPONENTS}) + set(GEM_${Gem_NAME}_FOUND TRUE) + list(APPEND components_found_vars GEM_${Gem_NAME}_FOUND) + # If the gem is installed as a gem + if(NOT GEM_OS_PKG) + set(GEM_HOME ENV{GEM_HOME}) + + # Use `gem content <gem-name>` to extract current information about installed gems + # Store the information into ${GEM_LOCAL_INFO} + EXECUTE_PROCESS(COMMAND ${GEM_EXECUTABLE} content ${Gem_NAME} + RESULT_VARIABLE GEM_RUN_RESULT + OUTPUT_VARIABLE GEM_LOCAL_INFO) + + if(GEM_RUN_RESULT STREQUAL "0") + list(APPEND FOUND_GEMS ${Gem_NAME}) + set(_library_NAME_PATTERN lib${Gem_NAME}.a + lib${Gem_NAME}.so + lib${Gem_NAME}.dylib + ${Gem_NAME}.a + ${Gem_NAME}.so + ${Gem_NAME}.dylib + .*.a + .*.so + .*.dylib + ) + + set(_header_SUFFIX_PATTERN + .h + .hh + .hpp + ) + + # Create a list from the output results of the gem command + string(REPLACE "\n" ";" GEM_CONTENT_LIST "${GEM_LOCAL_INFO}") + foreach(_gem_CONTENT_PATH ${GEM_CONTENT_LIST}) + + # Convert so that only '/' Unix path separator are being using + # needed to do proper regex matching + FILE(TO_CMAKE_PATH ${_gem_CONTENT_PATH} gem_CONTENT_PATH) + + # Identify library -- checking for a library in the gems 'lib' (sub)directory + # Search for an existing library, but only within the gems folder + foreach(_library_NAME ${_library_NAME_PATTERN}) + STRING(REGEX MATCH ".*${Gem_NAME}.*/lib/.*${_library_NAME}$" GEM_PATH_INFO "${gem_CONTENT_PATH}") + if(NOT "${GEM_PATH_INFO}" STREQUAL "") + list(APPEND GEM_LIBRARIES ${GEM_PATH_INFO}) + break() + endif() + endforeach() + + # Identify headers + # Checking for available headers in an include directory + foreach(_header_PATTERN ${_header_SUFFIX_PATTERN}) + STRING(REGEX MATCH ".*${Gem_NAME}.*/include/.*${_header_PATTERN}$" GEM_PATH_INFO "${gem_CONTENT_PATH}") + if(NOT "${GEM_PATH_INFO}" STREQUAL "") + STRING(REGEX REPLACE "(.*${Gem_NAME}.*/include/).*${_header_PATTERN}$" "\\1" GEM_PATH_INFO "${gem_CONTENT_PATH}") + list(APPEND GEM_INCLUDE_DIRS ${GEM_PATH_INFO}) + break() + endif() + endforeach() + endforeach() + else() + set(GEM_${Gem_NAME}_FOUND FALSE) + endif() + else(NOT GEM_OS_PKG) + pkg_check_modules(GEM_PKG ${Gem_NAME}) + set(GEM_${GEM_NAME}_FOUND GEM_PKG_FOUND) + set(GEM_INCLUDE_DIRS ${GEM_PKG_INCLUDE_DIRS}) + set(GEM_LIBRARIES ${GEM_PKG_LIBRARIES} ${GEM_PKG_STATIC_LIBRARIES}) + list(APPEND GEM_LIBRARIES ${GEM_PKG_LDFLAGS} ${GEM_PKG_STATIC_LDFLAGS}) + list(APPEND GEM_LIBRARIES ${GEM_PKG_LDFLAGS_OTHER} ${GEM_PKG_STATIC_LDFLAGS_OTHER}) + + if(GEM_DEBUG) + message(STATUS "GEM_OS_PKG is defined") + message(STATUS "GEM_INCLUDE_DIRS ${GEM_INCLUDE_DIRS}") + message(STATUS "GEM_STATIC_LIBRARY_DIRS ${GEM_PKG_STATIC_LIBRARY_DIRS}") + message(STATUS "GEM_LIBRARY_DIRS ${GEM_PKG_STATIC_LIBRARY_DIRS}") + message(STATUS "GEM_STATIC_LIBRARIES ${GEM_PKG_STATIC_LIBRARIES}") + message(STATUS "GEM_LIBRARIES ${GEM_LIBRARIES}") + endif() + endif() + + if(GEM_DEBUG) + message(STATUS "${Gem_NAME} library dir: ${GEM_LIBRARIES}") + message(STATUS "${Gem_NAME} include dir: ${GEM_INCLUDE_DIRS}") + endif() +endforeach() + +# Compact the lists +if(DEFINED GEM_LIBRARIES) + LIST(REMOVE_DUPLICATES GEM_LIBRARIES) +endif() +if(DEFINED GEM_INCLUDE_DIRS) + LIST(REMOVE_DUPLICATES GEM_INCLUDE_DIRS) +endif() + +find_package_handle_standard_args(Gem + REQUIRED_VARS ${components_found_vars} + FAIL_MESSAGE "Could not find all required gems") + diff --git a/libosmium/FindLZ4.cmake b/libosmium/FindLZ4.cmake new file mode 100644 index 0000000000000000000000000000000000000000..8c94e3bcd1e5e6bd236d52e62ae35480947abefe --- /dev/null +++ b/libosmium/FindLZ4.cmake @@ -0,0 +1,38 @@ +find_path(LZ4_INCLUDE_DIR + NAMES lz4.h + DOC "lz4 include directory") +mark_as_advanced(LZ4_INCLUDE_DIR) +find_library(LZ4_LIBRARY + NAMES lz4 liblz4 + DOC "lz4 library") +mark_as_advanced(LZ4_LIBRARY) + +if (LZ4_INCLUDE_DIR) + file(STRINGS "${LZ4_INCLUDE_DIR}/lz4.h" _lz4_version_lines + REGEX "#define[ \t]+LZ4_VERSION_(MAJOR|MINOR|RELEASE)") + string(REGEX REPLACE ".*LZ4_VERSION_MAJOR *\([0-9]*\).*" "\\1" _lz4_version_major "${_lz4_version_lines}") + string(REGEX REPLACE ".*LZ4_VERSION_MINOR *\([0-9]*\).*" "\\1" _lz4_version_minor "${_lz4_version_lines}") + string(REGEX REPLACE ".*LZ4_VERSION_RELEASE *\([0-9]*\).*" "\\1" _lz4_version_release "${_lz4_version_lines}") + set(LZ4_VERSION "${_lz4_version_major}.${_lz4_version_minor}.${_lz4_version_release}") + unset(_lz4_version_major) + unset(_lz4_version_minor) + unset(_lz4_version_release) + unset(_lz4_version_lines) +endif () + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(LZ4 + REQUIRED_VARS LZ4_LIBRARY LZ4_INCLUDE_DIR + VERSION_VAR LZ4_VERSION) + +if (LZ4_FOUND) + set(LZ4_INCLUDE_DIRS "${LZ4_INCLUDE_DIR}") + set(LZ4_LIBRARIES "${LZ4_LIBRARY}") + + if (NOT TARGET LZ4::LZ4) + add_library(LZ4::LZ4 UNKNOWN IMPORTED) + set_target_properties(LZ4::LZ4 PROPERTIES + IMPORTED_LOCATION "${LZ4_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${LZ4_INCLUDE_DIR}") + endif () +endif () diff --git a/libosmium/FindOsmium.cmake b/libosmium/FindOsmium.cmake new file mode 100644 index 0000000000000000000000000000000000000000..651c09a72b61f99ce3e452d8605ae69476f14ff8 --- /dev/null +++ b/libosmium/FindOsmium.cmake @@ -0,0 +1,366 @@ +#---------------------------------------------------------------------- +# +# FindOsmium.cmake +# +# Find the Libosmium headers and, optionally, several components needed +# for different Libosmium functions. +# +#---------------------------------------------------------------------- +# +# Usage: +# +# Copy this file somewhere into your project directory, where cmake can +# find it. Usually this will be a directory called "cmake" which you can +# add to the CMake module search path with the following line in your +# CMakeLists.txt: +# +# list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") +# +# Then add the following in your CMakeLists.txt: +# +# find_package(Osmium [version] REQUIRED COMPONENTS <XXX>) +# include_directories(SYSTEM ${OSMIUM_INCLUDE_DIRS}) +# +# The version number is optional. If it is not set, any version of +# libosmium will do. +# +# For the <XXX> substitute a space separated list of one or more of the +# following components: +# +# pbf - include libraries needed for PBF input and output +# xml - include libraries needed for XML input and output +# io - include libraries needed for any type of input/output +# geos - include if you want to use any of the GEOS functions +# gdal - include if you want to use any of the OGR functions +# proj - include if you want to use any of the Proj.4 functions +# sparsehash - include if you use the sparsehash index (deprecated!) +# lz4 - include support for LZ4 compression of PBF files +# +# You can check for success with something like this: +# +# if(NOT OSMIUM_FOUND) +# message(WARNING "Libosmium not found!\n") +# endif() +# +#---------------------------------------------------------------------- +# +# Variables: +# +# OSMIUM_FOUND - True if Osmium found. +# OSMIUM_INCLUDE_DIRS - Where to find include files. +# OSMIUM_XML_LIBRARIES - Libraries needed for XML I/O. +# OSMIUM_PBF_LIBRARIES - Libraries needed for PBF I/O. +# OSMIUM_IO_LIBRARIES - Libraries needed for XML or PBF I/O. +# OSMIUM_LIBRARIES - All libraries Osmium uses somewhere. +# +#---------------------------------------------------------------------- + +# This is the list of directories where we look for osmium includes. +set(_osmium_include_path + ../libosmium + ~/Library/Frameworks + /Library/Frameworks + /opt/local # DarwinPorts + /opt +) + +# Look for the header file. +find_path(OSMIUM_INCLUDE_DIR osmium/version.hpp + PATH_SUFFIXES include + PATHS ${_osmium_include_path} +) + +# Check libosmium version number +if(Osmium_FIND_VERSION) + if(NOT EXISTS "${OSMIUM_INCLUDE_DIR}/osmium/version.hpp") + message(FATAL_ERROR "Missing ${OSMIUM_INCLUDE_DIR}/osmium/version.hpp. Either your libosmium version is too old, or libosmium wasn't found in the place you said.") + endif() + file(STRINGS "${OSMIUM_INCLUDE_DIR}/osmium/version.hpp" _libosmium_version_define REGEX "#define LIBOSMIUM_VERSION_STRING") + if("${_libosmium_version_define}" MATCHES "#define LIBOSMIUM_VERSION_STRING \"([0-9.]+)\"") + set(_libosmium_version "${CMAKE_MATCH_1}") + else() + set(_libosmium_version "unknown") + endif() +endif() + +set(OSMIUM_INCLUDE_DIRS "${OSMIUM_INCLUDE_DIR}") + +#---------------------------------------------------------------------- +# +# Check for optional components +# +#---------------------------------------------------------------------- +if(Osmium_FIND_COMPONENTS) + foreach(_component ${Osmium_FIND_COMPONENTS}) + string(TOUPPER ${_component} _component_uppercase) + set(Osmium_USE_${_component_uppercase} TRUE) + endforeach() +endif() + +#---------------------------------------------------------------------- +# Component 'io' is an alias for 'pbf' and 'xml' +if(Osmium_USE_IO) + set(Osmium_USE_PBF TRUE) + set(Osmium_USE_XML TRUE) +endif() + +#---------------------------------------------------------------------- +# Component 'ogr' is an alias for 'gdal' +if(Osmium_USE_OGR) + set(Osmium_USE_GDAL TRUE) +endif() + +#---------------------------------------------------------------------- +# Component 'pbf' +if(Osmium_USE_PBF) + find_package(ZLIB) + find_package(Threads) + find_package(Protozero 1.6.3) + + if(Osmium_USE_LZ4) + find_package(LZ4 REQUIRED) + add_definitions(-DOSMIUM_WITH_LZ4) + endif() + + list(APPEND OSMIUM_EXTRA_FIND_VARS ZLIB_FOUND Threads_FOUND PROTOZERO_INCLUDE_DIR) + if(ZLIB_FOUND AND Threads_FOUND AND PROTOZERO_FOUND) + list(APPEND OSMIUM_PBF_LIBRARIES + ${ZLIB_LIBRARIES} + ${LZ4_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} + ) + list(APPEND OSMIUM_INCLUDE_DIRS + ${ZLIB_INCLUDE_DIR} + ${LZ4_INCLUDE_DIRS} + ${PROTOZERO_INCLUDE_DIR} + ) + else() + message(WARNING "Osmium: Can not find some libraries for PBF input/output, please install them or configure the paths.") + endif() +endif() + +#---------------------------------------------------------------------- +# Component 'xml' +if(Osmium_USE_XML) + find_package(EXPAT) + find_package(BZip2) + find_package(ZLIB) + find_package(Threads) + + list(APPEND OSMIUM_EXTRA_FIND_VARS EXPAT_FOUND BZIP2_FOUND ZLIB_FOUND Threads_FOUND) + if(EXPAT_FOUND AND BZIP2_FOUND AND ZLIB_FOUND AND Threads_FOUND) + list(APPEND OSMIUM_XML_LIBRARIES + ${EXPAT_LIBRARIES} + ${BZIP2_LIBRARIES} + ${ZLIB_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} + ) + list(APPEND OSMIUM_INCLUDE_DIRS + ${EXPAT_INCLUDE_DIR} + ${BZIP2_INCLUDE_DIR} + ${ZLIB_INCLUDE_DIR} + ) + else() + message(WARNING "Osmium: Can not find some libraries for XML input/output, please install them or configure the paths.") + endif() +endif() + +#---------------------------------------------------------------------- +list(APPEND OSMIUM_IO_LIBRARIES + ${OSMIUM_PBF_LIBRARIES} + ${OSMIUM_XML_LIBRARIES} +) + +list(APPEND OSMIUM_LIBRARIES + ${OSMIUM_IO_LIBRARIES} +) + +#---------------------------------------------------------------------- +# Component 'geos' +if(Osmium_USE_GEOS) + find_path(GEOS_INCLUDE_DIR geos/geom.h) + find_library(GEOS_LIBRARY NAMES geos) + + list(APPEND OSMIUM_EXTRA_FIND_VARS GEOS_INCLUDE_DIR GEOS_LIBRARY) + if(GEOS_INCLUDE_DIR AND GEOS_LIBRARY) + SET(GEOS_FOUND 1) + list(APPEND OSMIUM_LIBRARIES ${GEOS_LIBRARY}) + list(APPEND OSMIUM_INCLUDE_DIRS ${GEOS_INCLUDE_DIR}) + else() + message(WARNING "Osmium: GEOS library is required but not found, please install it or configure the paths.") + endif() +endif() + +#---------------------------------------------------------------------- +# Component 'gdal' (alias 'ogr') +if(Osmium_USE_GDAL) + find_package(GDAL) + + list(APPEND OSMIUM_EXTRA_FIND_VARS GDAL_FOUND) + if(GDAL_FOUND) + list(APPEND OSMIUM_LIBRARIES ${GDAL_LIBRARIES}) + list(APPEND OSMIUM_INCLUDE_DIRS ${GDAL_INCLUDE_DIRS}) + else() + message(WARNING "Osmium: GDAL library is required but not found, please install it or configure the paths.") + endif() +endif() + +#---------------------------------------------------------------------- +# Component 'proj' +if(Osmium_USE_PROJ) + find_path(PROJ_INCLUDE_DIR proj_api.h) + find_library(PROJ_LIBRARY NAMES proj) + + list(APPEND OSMIUM_EXTRA_FIND_VARS PROJ_INCLUDE_DIR PROJ_LIBRARY) + if(PROJ_INCLUDE_DIR AND PROJ_LIBRARY) + set(PROJ_FOUND 1) + list(APPEND OSMIUM_LIBRARIES ${PROJ_LIBRARY}) + list(APPEND OSMIUM_INCLUDE_DIRS ${PROJ_INCLUDE_DIR}) + else() + message(WARNING "Osmium: PROJ.4 library is required but not found, please install it or configure the paths.") + endif() +endif() + +#---------------------------------------------------------------------- +# Component 'sparsehash' +if(Osmium_USE_SPARSEHASH) + message(WARNING "Osmium: Use of Google SparseHash is deprecated. Please switch to a different index type.") + find_path(SPARSEHASH_INCLUDE_DIR google/sparsetable) + + list(APPEND OSMIUM_EXTRA_FIND_VARS SPARSEHASH_INCLUDE_DIR) + if(SPARSEHASH_INCLUDE_DIR) + # Find size of sparsetable::size_type. This does not work on older + # CMake versions because they can do this check only in C, not in C++. + if(NOT CMAKE_VERSION VERSION_LESS 3.0) + include(CheckTypeSize) + set(CMAKE_REQUIRED_INCLUDES ${SPARSEHASH_INCLUDE_DIR}) + set(CMAKE_EXTRA_INCLUDE_FILES "google/sparsetable") + check_type_size("google::sparsetable<int>::size_type" SPARSETABLE_SIZE_TYPE LANGUAGE CXX) + set(CMAKE_EXTRA_INCLUDE_FILES) + set(CMAKE_REQUIRED_INCLUDES) + else() + set(SPARSETABLE_SIZE_TYPE ${CMAKE_SIZEOF_VOID_P}) + endif() + + # Sparsetable::size_type must be at least 8 bytes (64bit), otherwise + # OSM object IDs will not fit. + if(SPARSETABLE_SIZE_TYPE GREATER 7) + set(SPARSEHASH_FOUND 1) + add_definitions(-DOSMIUM_WITH_SPARSEHASH=${SPARSEHASH_FOUND}) + list(APPEND OSMIUM_INCLUDE_DIRS ${SPARSEHASH_INCLUDE_DIR}) + else() + message(WARNING "Osmium: Disabled Google SparseHash library on 32bit system (size_type=${SPARSETABLE_SIZE_TYPE}).") + endif() + else() + message(WARNING "Osmium: Google SparseHash library is required but not found, please install it or configure the paths.") + endif() +endif() + +#---------------------------------------------------------------------- + +list(REMOVE_DUPLICATES OSMIUM_INCLUDE_DIRS) + +if(OSMIUM_XML_LIBRARIES) + list(REMOVE_DUPLICATES OSMIUM_XML_LIBRARIES) +endif() + +if(OSMIUM_PBF_LIBRARIES) + list(REMOVE_DUPLICATES OSMIUM_PBF_LIBRARIES) +endif() + +if(OSMIUM_IO_LIBRARIES) + list(REMOVE_DUPLICATES OSMIUM_IO_LIBRARIES) +endif() + +if(OSMIUM_LIBRARIES) + list(REMOVE_DUPLICATES OSMIUM_LIBRARIES) +endif() + +#---------------------------------------------------------------------- +# +# Check that all required libraries are available +# +#---------------------------------------------------------------------- +if(OSMIUM_EXTRA_FIND_VARS) + list(REMOVE_DUPLICATES OSMIUM_EXTRA_FIND_VARS) +endif() +# Handle the QUIETLY and REQUIRED arguments and the optional version check +# and set OSMIUM_FOUND to TRUE if all listed variables are TRUE. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Osmium + REQUIRED_VARS OSMIUM_INCLUDE_DIR ${OSMIUM_EXTRA_FIND_VARS} + VERSION_VAR _libosmium_version) +unset(OSMIUM_EXTRA_FIND_VARS) + +#---------------------------------------------------------------------- +# +# A function for setting the -pthread option in compilers/linkers +# +#---------------------------------------------------------------------- +function(set_pthread_on_target _target) + if(NOT MSVC) + set_target_properties(${_target} PROPERTIES COMPILE_FLAGS "-pthread") + if(NOT APPLE) + set_target_properties(${_target} PROPERTIES LINK_FLAGS "-pthread") + endif() + endif() +endfunction() + +#---------------------------------------------------------------------- +# +# Add compiler flags +# +#---------------------------------------------------------------------- +add_definitions(-D_LARGEFILE_SOURCE -D_FILE_OFFSET_BITS=64) + +if(MSVC) + add_definitions(-wd4996) + + # Disable warning C4068: "unknown pragma" because we want it to ignore + # pragmas for other compilers. + add_definitions(-wd4068) + + # Disable warning C4715: "not all control paths return a value" because + # it generates too many false positives. + add_definitions(-wd4715) + + # Disable warning C4351: new behavior: elements of array '...' will be + # default initialized. The new behaviour is correct and we don't support + # old compilers anyway. + add_definitions(-wd4351) + + # Disable warning C4503: "decorated name length exceeded, name was truncated" + # there are more than 150 of generated names in libosmium longer than 4096 symbols supported in MSVC + add_definitions(-wd4503) + + add_definitions(-DNOMINMAX -DWIN32_LEAN_AND_MEAN -D_CRT_SECURE_NO_WARNINGS) +endif() + +if(APPLE AND "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") +# following only available from cmake 2.8.12: +# add_compile_options(-stdlib=libc++) +# so using this instead: + add_definitions(-stdlib=libc++) + set(LDFLAGS ${LDFLAGS} -stdlib=libc++) +endif() + +#---------------------------------------------------------------------- + +# This is a set of recommended warning options that can be added when compiling +# libosmium code. +if(MSVC) + set(OSMIUM_WARNING_OPTIONS "/W3 /wd4514" CACHE STRING "Recommended warning options for libosmium") +else() + set(OSMIUM_WARNING_OPTIONS "-Wall -Wextra -pedantic -Wredundant-decls -Wdisabled-optimization -Wctor-dtor-privacy -Wnon-virtual-dtor -Woverloaded-virtual -Wsign-promo -Wold-style-cast" CACHE STRING "Recommended warning options for libosmium") +endif() + +set(OSMIUM_DRACONIC_CLANG_OPTIONS "-Wdocumentation -Wunused-exception-parameter -Wmissing-declarations -Weverything -Wno-c++98-compat -Wno-c++98-compat-pedantic -Wno-unused-macros -Wno-exit-time-destructors -Wno-global-constructors -Wno-padded -Wno-switch-enum -Wno-missing-prototypes -Wno-weak-vtables -Wno-cast-align -Wno-float-equal") + +if(Osmium_DEBUG) + message(STATUS "OSMIUM_XML_LIBRARIES=${OSMIUM_XML_LIBRARIES}") + message(STATUS "OSMIUM_PBF_LIBRARIES=${OSMIUM_PBF_LIBRARIES}") + message(STATUS "OSMIUM_IO_LIBRARIES=${OSMIUM_IO_LIBRARIES}") + message(STATUS "OSMIUM_LIBRARIES=${OSMIUM_LIBRARIES}") + message(STATUS "OSMIUM_INCLUDE_DIRS=${OSMIUM_INCLUDE_DIRS}") +endif() + diff --git a/libosmium/FindProtozero.cmake b/libosmium/FindProtozero.cmake new file mode 100644 index 0000000000000000000000000000000000000000..ad16cabeb00e4a2d6cb381cf083491ba24ebeb6f --- /dev/null +++ b/libosmium/FindProtozero.cmake @@ -0,0 +1,63 @@ +#---------------------------------------------------------------------- +# +# FindProtozero.cmake +# +# Find the protozero headers. +# +#---------------------------------------------------------------------- +# +# Usage: +# +# Copy this file somewhere into your project directory, where cmake can +# find it. Usually this will be a directory called "cmake" which you can +# add to the CMake module search path with the following line in your +# CMakeLists.txt: +# +# list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") +# +# Then add the following in your CMakeLists.txt: +# +# find_package(Protozero [version] [REQUIRED]) +# include_directories(SYSTEM ${PROTOZERO_INCLUDE_DIR}) +# +# The version number is optional. If it is not set, any version of +# protozero will do. +# +# if(NOT PROTOZERO_FOUND) +# message(WARNING "Protozero not found!\n") +# endif() +# +#---------------------------------------------------------------------- +# +# Variables: +# +# PROTOZERO_FOUND - True if Protozero was found. +# PROTOZERO_INCLUDE_DIR - Where to find include files. +# +#---------------------------------------------------------------------- + +# find include path +find_path(PROTOZERO_INCLUDE_DIR protozero/version.hpp + PATH_SUFFIXES include + PATHS ${CMAKE_SOURCE_DIR}/../protozero +) + +# Check version number +if(Protozero_FIND_VERSION) + file(STRINGS "${PROTOZERO_INCLUDE_DIR}/protozero/version.hpp" _version_define REGEX "#define PROTOZERO_VERSION_STRING") + if("${_version_define}" MATCHES "#define PROTOZERO_VERSION_STRING \"([0-9.]+)\"") + set(_version "${CMAKE_MATCH_1}") + else() + set(_version "unknown") + endif() +endif() + +#set(PROTOZERO_INCLUDE_DIRS "${PROTOZERO_INCLUDE_DIR}") + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Protozero + REQUIRED_VARS PROTOZERO_INCLUDE_DIR + VERSION_VAR _version) + + +#---------------------------------------------------------------------- diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b584288126167c5b52f30a06c9c342d218d19f69..8600f515f15e66c09b1c9399c18d8c5ccfb14372 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,12 +1,22 @@ # driver source files -SET(sources gradient.cpp g2_spline.cpp dijkstra.cpp opendrive_geometry.cpp opendrive_common.cpp opendrive_line.cpp opendrive_spiral.cpp opendrive_arc.cpp opendrive_param_poly3.cpp opendrive_signal.cpp opendrive_object.cpp opendrive_road.cpp opendrive_road_segment.cpp opendrive_lane.cpp opendrive_road_node.cpp opendrive_link.cpp) +SET(sources gradient.cpp g2_spline.cpp dijkstra.cpp common.cpp road.cpp junction.cpp road_segment.cpp road_map.cpp vel_profile.cpp vel_spline.cpp vel_trapezoid.cpp) +SET(opendrive_sources opendrive/opendrive_geometry.cpp opendrive/opendrive_line.cpp opendrive/opendrive_spiral.cpp opendrive/opendrive_arc.cpp opendrive/opendrive_param_poly3.cpp opendrive/opendrive_signal.cpp opendrive/opendrive_object.cpp opendrive/opendrive_road_segment.cpp opendrive/opendrive_lane.cpp opendrive/opendrive_junction.cpp) +SET(osm_sources osm/osm_map.cpp osm/osm_node.cpp osm/osm_way.cpp osm/osm_junction.cpp osm/osm_road_segment.cpp osm/osm_restriction.cpp) # application header files -SET(headers ../include/gradient.h ../include/g2_spline.h ../include/dijkstra.h ../include/opendrive_common.h ../include/opendrive_geometry.h ../include/opendrive_line.h ../include/opendrive_spiral.h ../include/opendrive_arc.h ../include/opendrive_param_poly3.h ../include/opendrive_signal.h ../include/opendrive_object.h ../include/opendrive_road.h ../include/opendrive_road_segment.h ../include/opendrive_lane.h ../include/opendrive_road_node.h ../include/opendrive_link.h) +SET(headers ../include/gradient.h ../include/g2_spline.h ../include/dijkstra.h ../include/common.h ../include/road.h ../include/junction.h ../include/road_segment.h ../include/road_map.h ../include/vel_profile.h ../include/vel_spline.h ../include/vel_trapezoid.h) +SET(opendrive_headers ../include/opendrive/opendrive_common.h ../include/opendrive/opendrive_geometry.h ../include/opendrive/opendrive_line.h ../include/opendrive/opendrive_spiral.h ../include/opendrive/opendrive_arc.h ../include/opendrive/opendrive_param_poly3.h ../include/opendrive/opendrive_signal.h ../include/opendrive/opendrive_object.h ../include/opendrive/opendrive_road_segment.h ../include/opendrive/opendrive_lane.h ../include/opendrive/opendrive_junction.h) +SET(osm_headers ../include/osm/osm_map.h ../include/osm/osm_node.h ../include/osm/osm_way.h ../include/osm/osm_junction.h ../include/osm/osm_road_segment.h ../include/osm/osm_restriction.h) + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/libosmium") + +set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib") # locate the necessary dependencies find_package(Eigen3 REQUIRED) find_package(iriutils REQUIRED) +find_package(Osmium REQUIRED COMPONENTS xml io gdal) +find_package(GeographicLib REQUIRED) ADD_SUBDIRECTORY(xml) @@ -15,12 +25,16 @@ INCLUDE_DIRECTORIES(../include) INCLUDE_DIRECTORIES(./) INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR}) +INCLUDE_DIRECTORIES(${OSMIUM_INCLUDE_DIRS}) +INCLUDE_DIRECTORIES(${GeographicLib_INCLUDE_DIRS}) # create the shared library -ADD_LIBRARY(${PROJECT_NAME} SHARED ${sources} ${XSD_SOURCES}) +ADD_LIBRARY(${PROJECT_NAME} SHARED ${sources} ${opendrive_sources} ${osm_sources} ${XSD_SOURCES}) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${XSD_LIBRARY}) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iriutils_LIBRARY}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OSMIUM_LIBRARIES}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GeographicLib_LIBRARIES}) ADD_DEPENDENCIES(${PROJECT_NAME} xsd_files_gen) @@ -32,6 +46,8 @@ INSTALL(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib/iri/${PROJECT_NAME} ARCHIVE DESTINATION lib/iri/${PROJECT_NAME}) INSTALL(FILES ${headers} DESTINATION include/iri/${PROJECT_NAME}) +INSTALL(FILES ${opendrive_headers} DESTINATION include/iri/${PROJECT_NAME}/opendrive) +INSTALL(FILES ${osm_headers} DESTINATION include/iri/${PROJECT_NAME}/osm) INSTALL(FILES ../Find${PROJECT_NAME}.cmake DESTINATION ${CMAKE_ROOT}/Modules/) #Install script files if needed diff --git a/src/opendrive_common.cpp b/src/common.cpp similarity index 94% rename from src/opendrive_common.cpp rename to src/common.cpp index c81c8ded5b9c5f084a80dadf53f4ef582f6a9c8c..57ab78d02d25937aadd3e109d8dcbf04ab8d6e74 100644 --- a/src/opendrive_common.cpp +++ b/src/common.cpp @@ -1,4 +1,5 @@ -#include "opendrive_common.h" +#include "common.h" + #include <cmath> double normalize_angle(double angle) @@ -27,3 +28,4 @@ double diff_angle(double angle1,double angle2) return std::fmin(diff1,diff2); } + diff --git a/src/dijkstra.cpp b/src/dijkstra.cpp index 985df9c6427e7a1b735a7fc9456846f7b71c4f26..e3089e29244c33660356ea4f3ef06a2632dfdcf3 100644 --- a/src/dijkstra.cpp +++ b/src/dijkstra.cpp @@ -55,7 +55,7 @@ double CDijkstra::find_shortest_path(Eigen::MatrixXd &graph,unsigned int start_n { Eigen::MatrixXd w=Eigen::MatrixXd::Zero(graph.rows(),graph.cols()); Eigen::MatrixXd d=Eigen::MatrixXd(graph.rows(),2); - Eigen::MatrixXd d2,new_d2; + Eigen::MatrixXd d2,new_d2,int_graph=graph; unsigned int i,j,k,l; path.clear(); @@ -64,24 +64,29 @@ double CDijkstra::find_shortest_path(Eigen::MatrixXd &graph,unsigned int start_n path.push_back(end_node); return 0.0; } + // arrange nodes + if(end_node==0) + end_node=start_node; + int_graph.row(0).swap(int_graph.row(start_node)); + int_graph.col(0).swap(int_graph.col(start_node)); // set Inf cost to all cells with 0 - for(i=0;i<graph.rows();i++) - for(j=0;j<graph.cols();j++) - if(graph(i,j)==0.0) - graph(i,j)=std::numeric_limits<double>::max()/2.0; - for(i=1;i<graph.rows();i++) + for(i=0;i<int_graph.rows();i++) + for(j=0;j<int_graph.cols();j++) + if(int_graph(i,j)==0.0) + int_graph(i,j)=std::numeric_limits<double>::max()/2.0; + for(i=1;i<int_graph.rows();i++) { w(0,i)=i; - w(1,i)=graph(0,i); + w(1,i)=int_graph(0,i); } - for(i=0;i<graph.rows();i++) + for(i=0;i<int_graph.rows();i++) { - d(i,0)=graph(0,i); + d(i,0)=int_graph(0,i); d(i,1)=i; } d2=d.block(1,0,d.rows()-1,2); l=1; - while(l<(graph.rows()-1)) + while(l<(int_graph.rows()-1)) { l++; this->sort_rows(d2); @@ -90,21 +95,23 @@ double CDijkstra::find_shortest_path(Eigen::MatrixXd &graph,unsigned int start_n new_d2=d2.block(1,0,d2.rows()-1,2); for(i=0;i<new_d2.rows();i++) { - if(d(new_d2(i,1),0)>(d(k,0)+graph(k,new_d2(i,1)))) + if(d(new_d2(i,1),0)>(d(k,0)+int_graph(k,new_d2(i,1)))) { - d(new_d2(i,1),0)=d(k,0)+graph(k,new_d2(i,1)); + d(new_d2(i,1),0)=d(k,0)+int_graph(k,new_d2(i,1)); new_d2(i,0)=d(new_d2(i,1),0); } } - for(i=1;i<graph.rows();i++) + for(i=1;i<int_graph.rows();i++) w(l,i)=d(i,0); d2=new_d2; } path.clear(); - // find the last reachable cell (preferably on the same target lane) - while(w(1,end_node)==w(w.rows()-1,end_node) && w(1,end_node)==std::numeric_limits<double>::max()/2.0) - end_node--; - path.push_back(end_node); + if(w(1,end_node)==w(w.rows()-1,end_node) && w(1,end_node)==std::numeric_limits<double>::max()/2.0) + return std::numeric_limits<double>::max()/2.0; + if(end_node==start_node) + path.push_back(0); + else + path.push_back(end_node); this->get_path(path,w,start_node,end_node); return w(w.rows()-1,end_node); diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 34527a7335814dc9a28efbfafd257298bb20f1da..2a1815eef6903f5637fe693d3c76663226e7b1b2 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -4,6 +4,16 @@ ADD_EXECUTABLE(dijkstra_test dijkstra_test.cpp) TARGET_LINK_LIBRARIES(dijkstra_test ${PROJECT_NAME}) # create an example application -ADD_EXECUTABLE(opendrive_test opendrive_test.cpp) +ADD_EXECUTABLE(build_from_scratch_test build_from_scratch_test.cpp) # link necessary libraries -TARGET_LINK_LIBRARIES(opendrive_test ${PROJECT_NAME}) +TARGET_LINK_LIBRARIES(build_from_scratch_test ${PROJECT_NAME}) + +# create an example application +ADD_EXECUTABLE(opendrive_import_test opendrive_import_test.cpp) +# link necessary libraries +TARGET_LINK_LIBRARIES(opendrive_import_test ${PROJECT_NAME}) + +# create an example application +ADD_EXECUTABLE(osm_import_test osm_import_test.cpp) +# link necessary libraries +TARGET_LINK_LIBRARIES(osm_import_test ${PROJECT_NAME}) diff --git a/src/examples/build_from_scratch_test.cpp b/src/examples/build_from_scratch_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8313acd4010b41b71fc5fb7dae45177b2ed4f4b8 --- /dev/null +++ b/src/examples/build_from_scratch_test.cpp @@ -0,0 +1,147 @@ +#include "road_map.h" +#include "exceptions.h" + +#include <iostream> + +int main(int argc, char *argv[]) +{ + try{ + CRoadMap map; + CRoad *road0,*road1,*road2,*road3,*road4,*road5; + CJunction *junction0,*junction1; + Eigen::MatrixXi connectivity; + std::vector<unsigned int> id_map; + std::vector<double> x,y,heading; + + road0=new CRoad(); + road0->set_num_lanes(2); + road0->set_lane_width(4.0); + road0->set_start_point(20.0,60.0,3.14159,0.0); + road0->add_segment(0.0,60.0,3.14159,0.0); + road0->add_segment(-20.0,40.0,4.71238898,0.0); + road0->add_segment(-20.0,20.0,4.71238898,0.0); + road0->add_segment(0.0,0.0,0.0,0.0); + road0->add_segment(20.0,0.0,0.0,0.0); + road1=new CRoad(); + road1->set_num_lanes(2); + road1->set_lane_width(4.0); + road1->set_start_point(20.0,0.0,3.14159,0.0); + road1->add_segment(0.0,0.0,3.14159,0.0); + road1->add_segment(-20.0,20.0,1.5707,0.0); + road1->add_segment(-20.0,40.0,1.5707,0.0); + road1->add_segment(0.0,60.0,0.0,0.0); + road1->add_segment(20.0,60.0,0.0,0.0); + road2=new CRoad(); + road2->set_num_lanes(2); + road2->set_lane_width(4.0); + road2->set_start_point(30.0,50.0,4.71238898,0.0); + road2->add_segment(30.0,10.0,4.71238898,0.0); + road3=new CRoad(); + road3->set_num_lanes(2); + road3->set_lane_width(4.0); + road3->set_start_point(30.0,10.0,1.5707,0.0); + road3->add_segment(30.0,50.0,1.5707,0.0); + road4=new CRoad(); + road4->set_num_lanes(2); + road4->set_lane_width(4.0); + road4->set_start_point(40.0,60.0,0.0,0.0); + road4->add_segment(60.0,60.0,0.0,0.0); + road4->add_segment(80.0,40.0,4.71238898,0.0); + road4->add_segment(80.0,20.0,4.71238898,0.0); + road4->add_segment(60.0,0.0,3.14159,0.0); + road4->add_segment(40.0,0.0,3.14159,0.0); + road5=new CRoad(); + road5->set_num_lanes(2); + road5->set_lane_width(4.0); + road5->set_start_point(40.0,0.0,0.0,0.0); + road5->add_segment(60.0,0.0,0.0,0.0); + road5->add_segment(80.0,20.0,1.5707,0.0); + road5->add_segment(80.0,40.0,1.5707,0.0); + road5->add_segment(60.0,60.0,3.14159,0.0); + road5->add_segment(40.0,60.0,3.14159,0.0); + + junction0=new CJunction(); + junction1=new CJunction(); + + map.add_road(road0); + map.add_road(road1); + map.add_road(road2); + map.add_road(road3); + map.add_road(road4); + map.add_road(road5); + + map.set_opposite_direction_roads_by_id(road0->get_id(),road1->get_id()); + map.set_opposite_direction_roads_by_id(road2->get_id(),road3->get_id()); + map.set_opposite_direction_roads_by_id(road4->get_id(),road5->get_id()); + + map.add_junction(junction0); + map.add_junction(junction1); + + junction0->add_incomming_road(road1); + junction0->add_incomming_road(road3); + junction0->add_incomming_road(road5); + junction0->add_outgoing_road(road0); + junction0->add_outgoing_road(road2); + junction0->add_outgoing_road(road4); + junction0->link_roads_by_id(road1->get_id(),road2->get_id()); + junction0->link_same_lanes_by_id(road1->get_id(),road2->get_id()); + junction0->link_roads_by_id(road1->get_id(),road4->get_id()); + junction0->link_same_lanes_by_id(road1->get_id(),road4->get_id()); + junction0->link_roads_by_id(road3->get_id(),road0->get_id()); + junction0->link_same_lanes_by_id(road3->get_id(),road0->get_id()); + junction0->link_roads_by_id(road3->get_id(),road4->get_id()); + junction0->link_same_lanes_by_id(road3->get_id(),road4->get_id()); + junction0->link_roads_by_id(road5->get_id(),road0->get_id()); + junction0->link_same_lanes_by_id(road5->get_id(),road0->get_id()); + junction0->link_roads_by_id(road5->get_id(),road2->get_id()); + junction0->link_same_lanes_by_id(road5->get_id(),road2->get_id()); + + junction1->add_incomming_road(road0); + junction1->add_incomming_road(road2); + junction1->add_incomming_road(road4); + junction1->add_outgoing_road(road1); + junction1->add_outgoing_road(road3); + junction1->add_outgoing_road(road5); + junction1->link_roads_by_id(road0->get_id(),road3->get_id()); + junction1->link_same_lanes_by_id(road0->get_id(),road3->get_id()); + junction1->link_roads_by_id(road0->get_id(),road5->get_id()); + junction1->link_same_lanes_by_id(road0->get_id(),road5->get_id()); + junction1->link_roads_by_id(road2->get_id(),road1->get_id()); + junction1->link_same_lanes_by_id(road2->get_id(),road1->get_id()); + junction1->link_roads_by_id(road2->get_id(),road5->get_id()); + junction1->link_same_lanes_by_id(road2->get_id(),road5->get_id()); + junction1->link_roads_by_id(road4->get_id(),road3->get_id()); + junction1->link_same_lanes_by_id(road4->get_id(),road3->get_id()); + junction1->link_roads_by_id(road4->get_id(),road1->get_id()); + junction1->link_same_lanes_by_id(road4->get_id(),road1->get_id()); + + map.save_opendrive("test_road.xodr"); + + map.get_lane_geometry(x,y,heading); + for(unsigned int i=0;i<x.size();i++) + std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl; + + return 0; + + map.get_segment_connectivity(connectivity,id_map); + for(unsigned int i=0;i<id_map.size();i++) + std::cout << i << " -> " << id_map[i] << " "; + std::cout << std::endl; + std::cout << "Initial connectivity:" << std::endl; + std::cout << connectivity << std::endl; + + map.remove_road_by_id(road3->get_id()); + + map.get_segment_connectivity(connectivity,id_map); + for(unsigned int i=0;i<id_map.size();i++) + std::cout << i << " -> " << id_map[i] << " "; + std::cout << std::endl; + std::cout << "Updated connectivity:" << std::endl; + std::cout << connectivity << std::endl; + } + catch (CException &e) + { + std::cout << "[Exception caught] : " << e.what() << std::endl; + } + return 0; +} diff --git a/src/examples/opendrive_build_road_test.cpp b/src/examples/opendrive_build_road_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..539310119c46877e3162f7bfacc8661ce57468d2 --- /dev/null +++ b/src/examples/opendrive_build_road_test.cpp @@ -0,0 +1,201 @@ +#include "opendrive_road.h" +#include "exceptions.h" +#include <iostream> +#include <vector> +#include <limits> + +int main(int argc, char *argv[]) +{ + std::vector<double> x,y; + COpendriveRoad road; + COpendriveRoadSegment *new_segment; + COpendriveLane new_lane(4.0,50.0,OD_MARK_SOLID); + TOpendriveWorldPose start_pose; + COpendriveLine *line_geom; + COpendriveArc *arc_geom; + COpendriveJunction *new_junction; + unsigned int road0_index,road1_index,road2_index,road3_index,road4_index,road5_index,road6_index,road7_index,road8_index,road9_index,road10_index,connection_index; + + try + { + road.set_resolution(0.1); + road.set_scale_factor(1.0); + road.set_min_road_length(0.1); + new_segment=new COpendriveRoadSegment("road0",OD_MARK_BROKEN,-1,false); + new_segment->add_left_lane(new_lane); + new_segment->add_right_lane(new_lane); + start_pose.x=0.0; + start_pose.y=0.0; + start_pose.heading=0.0; + line_geom=new COpendriveLine(start_pose,20.0); + new_segment->add_geometry(line_geom); + road0_index=road.add_segment(*new_segment); + delete new_segment; + delete line_geom; + new_segment=new COpendriveRoadSegment("road1",OD_MARK_BROKEN,-1,false); + new_segment->add_left_lane(new_lane); + new_segment->add_right_lane(new_lane); + start_pose.x=40.0; + start_pose.y=0.0; + start_pose.heading=0.0; + line_geom=new COpendriveLine(start_pose,20.0); + new_segment->add_geometry(line_geom); + road1_index=road.add_segment(*new_segment); + delete new_segment; + delete line_geom; + new_segment=new COpendriveRoadSegment("road2",OD_MARK_BROKEN,-1,false); + new_segment->add_left_lane(new_lane); + new_segment->add_right_lane(new_lane); + start_pose.x=60.0; + start_pose.y=0.0; + start_pose.heading=0.0; + arc_geom=new COpendriveArc(start_pose,20.0*90.0*3.14159/180.0,1.0/20.0); + new_segment->add_geometry(arc_geom); + road2_index=road.add_segment(*new_segment); + road.link_segments_by_index(road1_index,road2_index); + delete new_segment; + delete arc_geom; + new_segment=new COpendriveRoadSegment("road3",OD_MARK_BROKEN,-1,false); + new_segment->add_left_lane(new_lane); + new_segment->add_right_lane(new_lane); + start_pose.x=80.0; + start_pose.y=20.0; + start_pose.heading=1.5707; + line_geom=new COpendriveLine(start_pose,20.0); + new_segment->add_geometry(line_geom); + road3_index=road.add_segment(*new_segment); + road.link_segments_by_index(road2_index,road3_index); + delete new_segment; + delete line_geom; + new_segment=new COpendriveRoadSegment("road4",OD_MARK_BROKEN,-1,false); + new_segment->add_left_lane(new_lane); + new_segment->add_right_lane(new_lane); + start_pose.x=80.0; + start_pose.y=40.0; + start_pose.heading=1.5707; + arc_geom=new COpendriveArc(start_pose,20.0*90.0*3.14159/180.0,1.0/20.0); + new_segment->add_geometry(arc_geom); + road4_index=road.add_segment(*new_segment); + road.link_segments_by_index(road3_index,road4_index); + delete new_segment; + delete arc_geom; + new_segment=new COpendriveRoadSegment("road5",OD_MARK_BROKEN,-1,false); + new_segment->add_left_lane(new_lane); + new_segment->add_right_lane(new_lane); + start_pose.x=60.0; + start_pose.y=60.0; + start_pose.heading=3.14159; + line_geom=new COpendriveLine(start_pose,20.0); + new_segment->add_geometry(line_geom); + road5_index=road.add_segment(*new_segment); + road.link_segments_by_index(road4_index,road5_index); + delete new_segment; + delete line_geom; + new_segment=new COpendriveRoadSegment("road6",OD_MARK_BROKEN,-1,false); + new_segment->add_left_lane(new_lane); + new_segment->add_right_lane(new_lane); + start_pose.x=20.0; + start_pose.y=60.0; + start_pose.heading=3.14159; + line_geom=new COpendriveLine(start_pose,20.0); + new_segment->add_geometry(line_geom); + road6_index=road.add_segment(*new_segment); + delete new_segment; + delete line_geom; + new_segment=new COpendriveRoadSegment("road7",OD_MARK_BROKEN,-1,false); + new_segment->add_left_lane(new_lane); + new_segment->add_right_lane(new_lane); + start_pose.x=0.0; + start_pose.y=60.0; + start_pose.heading=3.14159; + arc_geom=new COpendriveArc(start_pose,20.0*90.0*3.14159/180.0,1.0/20.0); + new_segment->add_geometry(arc_geom); + road7_index=road.add_segment(*new_segment); + road.link_segments_by_index(road6_index,road7_index); + delete new_segment; + delete arc_geom; + new_segment=new COpendriveRoadSegment("road8",OD_MARK_BROKEN,-1,false); + new_segment->add_left_lane(new_lane); + new_segment->add_right_lane(new_lane); + start_pose.x=-20.0; + start_pose.y=40.0; + start_pose.heading=4.71229; + line_geom=new COpendriveLine(start_pose,20.0); + new_segment->add_geometry(line_geom); + road8_index=road.add_segment(*new_segment); + road.link_segments_by_index(road7_index,road8_index); + delete new_segment; + delete line_geom; + new_segment=new COpendriveRoadSegment("road8",OD_MARK_BROKEN,-1,false); + new_segment->add_left_lane(new_lane); + new_segment->add_right_lane(new_lane); + start_pose.x=-20.0; + start_pose.y=20.0; + start_pose.heading=4.71229; + arc_geom=new COpendriveArc(start_pose,20.0*90.0*3.14159/180.0,1.0/20.0); + new_segment->add_geometry(arc_geom); + road9_index=road.add_segment(*new_segment); + road.link_segments_by_index(road8_index,road9_index); + road.link_segments_by_index(road9_index,road0_index); + delete new_segment; + delete arc_geom; + new_segment=new COpendriveRoadSegment("road10",OD_MARK_BROKEN,-1,false); + new_segment->add_right_lane(new_lane); + start_pose.x=32.0; + start_pose.y=50.0; + start_pose.heading=4.71229; + line_geom=new COpendriveLine(start_pose,40.0); + new_segment->add_geometry(line_geom); + road10_index=road.add_segment(*new_segment); + delete new_segment; + delete line_geom; + // create junction roads + new_junction=new COpendriveJunction("junction1"); + connection_index=new_junction->add_connection(road0_index,road1_index,false); + new_junction->add_link_to_connection(connection_index,-1,-1); + connection_index=new_junction->add_connection(road1_index,road0_index,false); + new_junction->add_link_to_connection(connection_index,1,1); + connection_index=new_junction->add_connection(road10_index,road0_index,false); + new_junction->add_link_to_connection(connection_index,-1,1); + connection_index=new_junction->add_connection(road10_index,road1_index,false); + new_junction->add_link_to_connection(connection_index,-1,-1); + road.add_junction(*new_junction); + delete new_junction; + new_junction=new COpendriveJunction("junction2"); + connection_index=new_junction->add_connection(road5_index,road6_index,false); + new_junction->add_link_to_connection(connection_index,-1,-1); + connection_index=new_junction->add_connection(road6_index,road5_index,false); + new_junction->add_link_to_connection(connection_index,1,1); + connection_index=new_junction->add_connection(road6_index,road10_index,false); + new_junction->add_link_to_connection(connection_index,1,-1); + connection_index=new_junction->add_connection(road5_index,road10_index,false); + new_junction->add_link_to_connection(connection_index,-1,-1); + road.add_junction(*new_junction); + delete new_junction; + + road.save("./new_road.xodr"); + + for(unsigned int i=0;i<road.get_num_nodes();i++) + { + const COpendriveRoadNode &node=road.get_node(i); + for(unsigned int j=0;j<node.get_num_links();j++) + { + const COpendriveLink &link=node.get_link(j); + link.get_trajectory(x,y); + for(unsigned int k=0;k<x.size();k++) + std::cout << x[k] << "," << y[k] << std::endl; + } + } + + return 0; + + std::cout << road << std::endl; + return 0; + + } + catch (CException &e) + { + std::cout << "[Exception caught] : " << e.what() << std::endl; + } + return 0; +} diff --git a/src/examples/opendrive_import_test.cpp b/src/examples/opendrive_import_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4e3af3241634f516d235b3442b56c8d379401ced --- /dev/null +++ b/src/examples/opendrive_import_test.cpp @@ -0,0 +1,44 @@ +#include "road_map.h" +#include "exceptions.h" + +#include <iostream> + +//const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr"; +const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/build/test_road.xodr"; + +int main(int argc, char *argv[]) +{ + try{ + CRoadMap map,new_map; + Eigen::MatrixXi connectivity; + std::vector<unsigned int> id_map,new_path,old_path; + std::vector<double> x,y,heading; + + map.load_opendrive(opendrive_file); + + + map.get_lane_geometry(x,y,heading); + for(unsigned int i=0;i<x.size();i++) + std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl; + + return 0; + +// old_path.push_back(3); +// old_path.push_back(4); + old_path.push_back(22); +// old_path.push_back(10); + new_path=map.get_path_sub_roadmap(old_path,new_map); + + new_map.get_segment_connectivity(connectivity,id_map); + for(unsigned int i=0;i<id_map.size();i++) + std::cout << i << " -> " << id_map[i] << " "; + std::cout << std::endl; + std::cout << "Initial connectivity:" << std::endl; + std::cout << connectivity << std::endl; + } + catch (CException &e) + { + std::cout << "[Exception caught] : " << e.what() << std::endl; + } + return 0; +} diff --git a/src/examples/opendrive_test.cpp b/src/examples/opendrive_test.cpp deleted file mode 100644 index cff26ce8f6c5113ab29133f5cac4441e1f0628fd..0000000000000000000000000000000000000000 --- a/src/examples/opendrive_test.cpp +++ /dev/null @@ -1,217 +0,0 @@ -#include "opendrive_road.h" -#include "exceptions.h" -#include <iostream> -#include <vector> -#include <limits> - -int main(int argc, char *argv[]) -{ -// TOpendriveWorldPose closest_pose; - std::vector<double> x,y; - COpendriveRoad road,new_road,new_road2; - std::vector<unsigned int> path; - TOpendriveWorldPose end_pose,start_pose; - double length=5.0; -// std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/add_road_full.xodr"; - std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr"; - - try - { - road.set_resolution(0.1); - road.set_scale_factor(1.0); - road.set_min_road_length(0.1); - road.load(xml_file); -// std::cout << road << std::endl; - - for(unsigned int i=0;i<road.get_num_nodes();i++) - { - const COpendriveRoadNode &node=road.get_node(i); - for(unsigned int j=0;j<node.get_num_links();j++) - { - const COpendriveLink &link=node.get_link(j); - link.get_trajectory(x,y); - for(unsigned int k=0;k<x.size();k++) - std::cout << x[k] << "," << y[k] << std::endl; - } - } - return 0; -/* - path.push_back(4); - start_pose.x=10.0; - start_pose.y=0.0; - start_pose.heading=0.0; - end_pose.x=17.0; - end_pose.y=0.0; - end_pose.heading=0.0; - road.get_sub_road(path,end_pose,new_road); - std::cout << "road1" << std::endl; - std::cout << new_road << std::endl; -*/ - /* - path.clear(); - path.push_back(4); - path.push_back(6); - path.push_back(8); - path.push_back(150); - path.push_back(67); - path.push_back(69); - path.push_back(71); - start_pose.x=15.0; - start_pose.y=0.0; - start_pose.heading=0.0; - end_pose.x=18.5; - end_pose.y=22.0; - end_pose.heading=3.14159; - */ - /* - path.clear(); - path.push_back(61); - path.push_back(63); - path.push_back(65); - path.push_back(153); - path.push_back(10); - path.push_back(12); - path.push_back(14); - end_pose.x=15.0; - end_pose.y=0.0; - end_pose.heading=3.14159; - start_pose.x=18.5; - start_pose.y=22.0; - start_pose.heading=0.0; - */ - - path.clear(); - path.push_back(0); - path.push_back(98); - path.push_back(100); - path.push_back(16); - path.push_back(17); - path.push_back(18); - path.push_back(133); - path.push_back(134); - path.push_back(59); - path.push_back(157); - path.push_back(158); - path.push_back(11); - path.push_back(13); - path.push_back(15); - end_pose.x=9.5; - end_pose.y=0.6; - end_pose.heading=3.14159; - start_pose.x=6.7; - start_pose.y=2.3; - start_pose.heading=1.5707; - - road.get_sub_road(path,end_pose,new_road); -// std::cout << "road2" << std::endl; -// std::cout << new_road << std::endl; -/* - for(unsigned int i=0;i<new_road.get_num_nodes();i++) - { - const COpendriveRoadNode &node=new_road.get_node(i); - for(unsigned int j=0;j<node.get_num_links();j++) - { - const COpendriveLink &link=node.get_link(j); - link.get_trajectory(x,y); - for(unsigned int k=0;k<x.size();k++) - std::cout << x[k] << "," << y[k] << std::endl; - } - } -*/ - - length=21.0; - new_road.get_sub_road(start_pose,length,1.0,new_road2); -// std::cout << "road2" << std::endl; -// std::cout << new_road2 << std::endl; - for(unsigned int i=0;i<new_road2.get_num_nodes();i++) - { - const COpendriveRoadNode &node=new_road2.get_node(i); - for(unsigned int j=0;j<node.get_num_links();j++) - { - const COpendriveLink &link=node.get_link(j); - link.get_trajectory(x,y); - for(unsigned int k=0;k<x.size();k++) - std::cout << x[k] << "," << y[k] << std::endl; - } - } -/* - if(argc!=4) - { - std::cout << "Invalid number of parameters" << std::endl; - std::cout << argv[0] << " x y heading" << std::endl; - return -1; - } - end_pose.x=std::stod(argv[1]); - end_pose.y=std::stod(argv[2]); - end_pose.heading=std::stod(argv[3]); - length=road.get_closest_pose(end_pose,closest_pose); - std::cout << "closest pose to: " << x << "," << y << "," << heading << " -> " << closest_pose.x << "," << closest_pose.y << "," << closest_pose.heading << " at " << length << " m" << std::endl; -*/ -/* - path.push_back(0); - start_pose.x=1.0; - start_pose.y=0.0; - start_pose.heading=0.0; - end_pose.x=2.0; - end_pose.y=0.0; - end_pose.heading=0.0; - road.get_sub_road(path,end_pose,new_road); - std::cout << "road1" << std::endl; - std::cout << new_road << std::endl; -*/ -/* - path.clear(); - path.push_back(0); - path.push_back(22); - path.push_back(2); - start_pose.x=1.0; - start_pose.y=0.0; - start_pose.heading=0.0; - end_pose.x=5.0; - end_pose.y=0.0; - end_pose.heading=0.0; - road.get_sub_road(path,end_pose,new_road); - std::cout << "road2" << std::endl; - std::cout << new_road << std::endl; - path.clear(); - path.push_back(0); - path.push_back(22); - path.push_back(2); - path.push_back(4); - path.push_back(6); - path.push_back(8); - path.push_back(10); - path.push_back(25); - path.push_back(21); - path.push_back(24); - path.push_back(1); - start_pose.x=1.0; - start_pose.y=0.0; - start_pose.heading=0.0; - end_pose.x=2.0; - end_pose.y=0.0; - end_pose.heading=0.0; - road.get_sub_road(path,end_pose,new_road); - std::cout << "road3" << std::endl; - std::cout << new_road << std::endl; -*/ -/* - for(unsigned int i=0;i<new_road.get_num_nodes();i++) - { - const COpendriveRoadNode &node=new_road.get_node(i); - for(unsigned int j=0;j<node.get_num_links();j++) - { - const COpendriveLink &link=node.get_link(j); - link.get_trajectory(x,y); - for(unsigned int k=0;k<x.size();k++) - std::cout << x[k] << "," << y[k] << std::endl; - } - } -*/ - } - catch (CException &e) - { - std::cout << "[Exception caught] : " << e.what() << std::endl; - } - return 0; -} diff --git a/src/examples/osm_import_test.cpp b/src/examples/osm_import_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4d15f586aabc7f31fac924ad24233caf2f9ed2d1 --- /dev/null +++ b/src/examples/osm_import_test.cpp @@ -0,0 +1,51 @@ +#include "road_map.h" +#include "exceptions.h" + +#include <iostream> + +//const std::string osm_file="/home/shernand/Downloads/test_osm.osm"; +//const std::string osm_file="/home/shernand/Downloads/test_osm2.osm"; +const std::string osm_file="/home/shernand/Downloads/diagonal2.osm"; + +int main(int argc, char *argv[]) +{ + try{ + CRoadMap map,new_map; + Eigen::MatrixXi connectivity; + std::vector<unsigned int> id_map,new_path,old_path; + std::vector<double> x,y,heading; + + map.load_osm(osm_file); + + /* + map.get_segment_geometry(x,y,heading); + for(unsigned int i=0;i<x.size();i++) + std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl; + + return 0; +*/ + map.get_lane_geometry(x,y,heading); + for(unsigned int i=0;i<x.size();i++) + std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl; + + return 0; + +// old_path.push_back(3); +// old_path.push_back(4); + old_path.push_back(22); +// old_path.push_back(10); + new_path=map.get_path_sub_roadmap(old_path,new_map); + + new_map.get_segment_connectivity(connectivity,id_map); + for(unsigned int i=0;i<id_map.size();i++) + std::cout << i << " -> " << id_map[i] << " "; + std::cout << std::endl; + std::cout << "Initial connectivity:" << std::endl; + std::cout << connectivity << std::endl; + } + catch (CException &e) + { + std::cout << "[Exception caught] : " << e.what() << std::endl; + } + return 0; +} diff --git a/src/g2_spline.cpp b/src/g2_spline.cpp index 9f21ff78878ac5c743a1560be2ec4f9090d98d49..2cd609be84289562c88e05ca1f9b157439572157 100644 --- a/src/g2_spline.cpp +++ b/src/g2_spline.cpp @@ -124,8 +124,8 @@ double CG2Spline::find_parameter(double length) index=length/this->resolution; - if(this->length[index]>length) - while(index>0 && this->length[index]>length) + if(this->length[index]-length>this->resolution) + while(index>0 && this->length[index]-length>this->resolution) index--; else while(index<this->num_points && this->length[index]<length) @@ -584,40 +584,42 @@ void CG2Spline::get_end_point(TPoint &point) void CG2Spline::generate(double resolution,unsigned int iterations) { - double l_c_start,l2_k_s_start,l_s_start,l2_k_c_start; - double l_c_end,l2_k_s_end,l_s_end,l2_k_c_end; + double n1_c_start,n1_2_k_s_start,n1_s_start,n1_2_k_c_start; + double n2_c_end,n2_2_k_s_end,n2_s_end,n2_2_k_c_end; double u,pow_u,new_length,last_length; - unsigned int i; + unsigned int i,max_num_points; this->resolution=resolution; last_length=0.0; new_length=sqrt(pow(this->end.x-this->start.x,2)+pow(this->end.y-this->start.y,2)); + max_num_points=10.0*(new_length/resolution); + this->num_points=0; if(new_length>this->resolution) { - while(iterations>0 && fabs(new_length-last_length)>this->resolution) + while(iterations>0 && fabs(new_length-last_length)>this->resolution && this->num_points<max_num_points) { last_length=new_length; this->num_points=ceil(last_length/resolution); - l_c_start=last_length*cos(this->start.heading); - l2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading); - l_c_end=last_length*cos(this->end.heading); - l2_k_s_end=pow(last_length,2)*this->end.curvature*sin(this->end.heading); + n1_c_start=last_length*cos(this->start.heading); + n1_2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading); + n2_c_end=last_length*cos(this->end.heading); + n2_2_k_s_end=pow(last_length,2)*this->end.curvature*sin(this->end.heading); this->x_coeff[0]=this->start.x; - this->x_coeff[1]=l_c_start; - this->x_coeff[2]=-0.5*l2_k_s_start; - this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*l_c_start-4.0*l_c_end+1.5*l2_k_s_start-0.5*l2_k_s_end; - this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*l_c_start+7.0*l_c_end-1.5*l2_k_s_start+l2_k_s_end; - this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*l_c_start-3.0*l_c_end+0.5*l2_k_s_start-0.5*l2_k_s_end; - l_s_start=last_length*sin(this->start.heading); - l2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading); - l_s_end=last_length*sin(this->end.heading); - l2_k_c_end=pow(last_length,2)*this->end.curvature*cos(this->end.heading); + this->x_coeff[1]=n1_c_start; + this->x_coeff[2]=-0.5*n1_2_k_s_start; + this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*n1_c_start-4.0*n2_c_end+1.5*n1_2_k_s_start-0.5*n2_2_k_s_end; + this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*n1_c_start+7.0*n2_c_end-1.5*n1_2_k_s_start+n2_2_k_s_end; + this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*n1_c_start-3.0*n2_c_end+0.5*n1_2_k_s_start-0.5*n2_2_k_s_end; + n1_s_start=last_length*sin(this->start.heading); + n1_2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading); + n2_s_end=last_length*sin(this->end.heading); + n2_2_k_c_end=pow(last_length,2)*this->end.curvature*cos(this->end.heading); this->y_coeff[0]=this->start.y; - this->y_coeff[1]=l_s_start; - this->y_coeff[2]=0.5*l2_k_c_start; - this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*l_s_start-4.0*l_s_end-1.5*l2_k_c_start+0.5*l2_k_c_end; - this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*l_s_start+7.0*l_s_end+1.5*l2_k_c_start-l2_k_c_end; - this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*l_s_start-3.0*l_s_end-0.5*l2_k_c_start+0.5*l2_k_c_end; + this->y_coeff[1]=n1_s_start; + this->y_coeff[2]=0.5*n1_2_k_c_start; + this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*n1_s_start-4.0*n2_s_end-1.5*n1_2_k_c_start+0.5*n2_2_k_c_end; + this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*n1_s_start+7.0*n2_s_end+1.5*n1_2_k_c_start-n2_2_k_c_end; + this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*n1_s_start-3.0*n2_s_end-0.5*n1_2_k_c_start+0.5*n2_2_k_c_end; new_length=0.0; if(this->x!=NULL) delete[] this->x; @@ -677,40 +679,42 @@ void CG2Spline::generate(double resolution,unsigned int iterations) void CG2Spline::generate(double resolution,double length,unsigned int iterations) { - double l_c_start,l2_k_s_start,l_s_start,l2_k_c_start; - double l_c_end,l2_k_s_end,l_s_end,l2_k_c_end; + double n1_c_start,n1_2_k_s_start,n1_s_start,n1_2_k_c_start; + double n2_c_end,n2_2_k_s_end,n2_s_end,n2_2_k_c_end; double u,pow_u,new_length,last_length; - unsigned int i; + unsigned int i,max_num_points; this->resolution=resolution; last_length=0.0; new_length=length; + max_num_points=10.0*(length/resolution); + this->num_points=0; if(new_length>this->resolution) { - while(iterations>0 && fabs(new_length-last_length)>this->resolution) + while(iterations>0 && fabs(new_length-last_length)>this->resolution && this->num_points<max_num_points) { last_length=new_length; this->num_points=ceil(last_length/resolution); - l_c_start=last_length*cos(this->start.heading); - l2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading); - l_c_end=last_length*cos(this->end.heading); - l2_k_s_end=pow(last_length,2)*this->end.curvature*sin(this->end.heading); + n1_c_start=last_length*cos(this->start.heading); + n1_2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading); + n2_c_end=last_length*cos(this->end.heading); + n2_2_k_s_end=pow(last_length,2)*this->end.curvature*sin(this->end.heading); this->x_coeff[0]=this->start.x; - this->x_coeff[1]=l_c_start; - this->x_coeff[2]=-0.5*l2_k_s_start; - this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*l_c_start-4.0*l_c_end+1.5*l2_k_s_start-0.5*l2_k_s_end; - this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*l_c_start+7.0*l_c_end-1.5*l2_k_s_start+l2_k_s_end; - this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*l_c_start-3.0*l_c_end+0.5*l2_k_s_start-0.5*l2_k_s_end; - l_s_start=last_length*sin(this->start.heading); - l2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading); - l_s_end=last_length*sin(this->end.heading); - l2_k_c_end=pow(last_length,2)*this->end.curvature*cos(this->end.heading); + this->x_coeff[1]=n1_c_start; + this->x_coeff[2]=-0.5*n1_2_k_s_start; + this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*n1_c_start-4.0*n2_c_end+1.5*n1_2_k_s_start-0.5*n2_2_k_s_end; + this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*n1_c_start+7.0*n2_c_end-1.5*n1_2_k_s_start+n2_2_k_s_end; + this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*n1_c_start-3.0*n2_c_end+0.5*n1_2_k_s_start-0.5*n2_2_k_s_end; + n1_s_start=last_length*sin(this->start.heading); + n1_2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading); + n2_s_end=last_length*sin(this->end.heading); + n2_2_k_c_end=pow(last_length,2)*this->end.curvature*cos(this->end.heading); this->y_coeff[0]=this->start.y; - this->y_coeff[1]=l_s_start; - this->y_coeff[2]=0.5*l2_k_c_start; - this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*l_s_start-4.0*l_s_end-1.5*l2_k_c_start+0.5*l2_k_c_end; - this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*l_s_start+7.0*l_s_end+1.5*l2_k_c_start-l2_k_c_end; - this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*l_s_start-3.0*l_s_end-0.5*l2_k_c_start+0.5*l2_k_c_end; + this->y_coeff[1]=n1_s_start; + this->y_coeff[2]=0.5*n1_2_k_c_start; + this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*n1_s_start-4.0*n2_s_end-1.5*n1_2_k_c_start+0.5*n2_2_k_c_end; + this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*n1_s_start+7.0*n2_s_end+1.5*n1_2_k_c_start-n2_2_k_c_end; + this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*n1_s_start-3.0*n2_s_end-0.5*n1_2_k_c_start+0.5*n2_2_k_c_end; new_length=0.0; if(this->x!=NULL) delete[] this->x; @@ -768,9 +772,113 @@ void CG2Spline::generate(double resolution,double length,unsigned int iterations this->generated=true; } -TPoint CG2Spline::evaluate(double length) +void CG2Spline::generate(double &resolution,double n1,double n2, double n3, double n4) { - return this->evaluate_parameter(this->find_parameter(length)); + double n1_c_start,n1_2_k_s_start,n1_s_start,n1_2_k_c_start; + double n3_c_start,n3_s_start; + double n2_c_end,n2_2_k_s_end,n2_s_end,n2_2_k_c_end; + double n4_c_end,n4_s_end; + double u,pow_u,length,new_length; + unsigned int i; + + this->resolution=resolution; + length=sqrt(pow(this->end.x-this->start.x,2)+pow(this->end.y-this->start.y,2)); + if(length>this->resolution) + { + this->num_points=ceil(length/resolution); + n1_c_start=n1*cos(this->start.heading); + n3_c_start=n3*cos(this->start.heading); + n1_2_k_s_start=pow(n1,2)*this->start.curvature*sin(this->start.heading); + n2_c_end=n2*cos(this->end.heading); + n4_c_end=n4*cos(this->end.heading); + n2_2_k_s_end=pow(n2,2)*this->end.curvature*sin(this->end.heading); + this->x_coeff[0]=this->start.x; + this->x_coeff[1]=n1_c_start; + this->x_coeff[2]=0.5*n3_c_start-0.5*n1_2_k_s_start; + this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*n1_c_start-1.5*n3_c_start-4.0*n2_c_end+0.5*n4_c_end+1.5*n1_2_k_s_start-0.5*n2_2_k_s_end; + this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*n1_c_start+1.5*n3_c_start+7.0*n2_c_end-n4_c_end-1.5*n1_2_k_s_start+n2_2_k_s_end; + this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*n1_c_start-0.5*n3_c_start-3.0*n2_c_end+0.5*n4_c_end+0.5*n1_2_k_s_start-0.5*n2_2_k_s_end; + n1_s_start=n1*sin(this->start.heading); + n3_s_start=n3*sin(this->start.heading); + n1_2_k_c_start=pow(n1,2)*this->start.curvature*cos(this->start.heading); + n2_s_end=n2*sin(this->end.heading); + n4_s_end=n4*sin(this->end.heading); + n2_2_k_c_end=pow(n2,2)*this->end.curvature*cos(this->end.heading); + this->y_coeff[0]=this->start.y; + this->y_coeff[1]=n1_s_start; + this->y_coeff[2]=0.5*n3_s_start+0.5*n1_2_k_c_start; + this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*n1_s_start-1.5*n3_s_start-4.0*n2_s_end+0.5*n4_s_end-1.5*n1_2_k_c_start+0.5*n2_2_k_c_end; + this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*n1_s_start+1.5*n3_s_start+7.0*n2_s_end-n4_s_end+1.5*n1_2_k_c_start-n2_2_k_c_end; + this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*n1_s_start-0.5*n3_s_start-3.0*n2_s_end+0.5*n4_s_end-0.5*n1_2_k_c_start+0.5*n2_2_k_c_end; + if(this->x!=NULL) + delete[] this->x; + this->x=new double[num_points]; + if(this->y!=NULL) + delete[] this->y; + this->y=new double[num_points]; + if(this->length!=NULL) + delete[] this->length; + this->length=new double[num_points]; + this->x[0]=this->x_coeff[0]; + this->y[0]=this->y_coeff[0]; + this->length[0]=0.0; + new_length=0.0; + for(i=1,u=(1.0/(num_points-1));i<num_points;i++,u+=(1.0/(num_points-1))) + { + this->x[i]=this->x_coeff[0]; + this->y[i]=this->y_coeff[0]; + pow_u=u; + this->x[i]+=this->x_coeff[1]*pow_u; + this->y[i]+=this->y_coeff[1]*pow_u; + pow_u*=u; + this->x[i]+=this->x_coeff[2]*pow_u; + this->y[i]+=this->y_coeff[2]*pow_u; + pow_u*=u; + this->x[i]+=this->x_coeff[3]*pow_u; + this->y[i]+=this->y_coeff[3]*pow_u; + pow_u*=u; + this->x[i]+=this->x_coeff[4]*pow_u; + this->y[i]+=this->y_coeff[4]*pow_u; + pow_u*=u; + this->x[i]+=this->x_coeff[5]*pow_u; + this->y[i]+=this->y_coeff[5]*pow_u; + new_length+=sqrt(pow(this->x[i]-this->x[i-1],2)+pow(this->y[i]-this->y[i-1],2)); + this->length[i]=new_length; + } + this->resolution=new_length/num_points; + resolution=this->resolution; + } + else + { + this->num_points=1; + if(this->x!=NULL) + delete[] this->x; + this->x=new double[num_points]; + x[0]=this->start.x; + if(this->y!=NULL) + delete[] this->y; + this->y=new double[num_points]; + y[0]=this->start.y; + if(this->length!=NULL) + delete[] this->length; + this->length=new double[num_points]; + this->length[0]=0.0; + } + this->generated=true; +} + +bool CG2Spline::evaluate(double length, TPoint& point) +{ + if(!this->generated) + this->generate(DEFAULT_RESOLUTION); + if (length < -this->resolution || length > (this->length[this->num_points-1]+this->resolution)) + return false; + else if(length<0.0) + length=0.0; + else if(length>this->length[this->num_points-1]) + length=this->length[this->num_points-1]; + point = this->evaluate_parameter(this->find_parameter(length)); + return true; } void CG2Spline::evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading) @@ -872,6 +980,7 @@ double CG2Spline::get_max_curvature_der(double max_error,unsigned int max_iter) double CG2Spline::find_closest_point(double x, double y, TPoint &point,double max_error,unsigned int max_iter) { + unsigned int point_index; double start_point; bool beyond_limits; @@ -885,8 +994,14 @@ double CG2Spline::find_closest_point(double x, double y, TPoint &point,double ma if(!beyond_limits) { point=this->evaluate_parameter(this->distance_grad.get_coordinate()); + point_index=(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1)); if(this->num_points>0) - return this->length[(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1))]; + { + if(point_index>=this->num_points) + return this->length[this->num_points-1]; + else + return this->length[point_index]; + } else return 0.0; } @@ -897,6 +1012,7 @@ double CG2Spline::find_closest_point(double x, double y, TPoint &point,double ma double CG2Spline::find_closest_point(double x, double y,double max_error,unsigned int max_iter) { + unsigned int point_index; double start_point; bool beyond_limits; @@ -909,8 +1025,14 @@ double CG2Spline::find_closest_point(double x, double y,double max_error,unsigne { if(!beyond_limits) { + point_index=(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1)); if(this->num_points) - return this->length[(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1))]; + { + if(point_index>=this->num_points) + return this->length[this->num_points-1]; + else + return this->length[point_index]; + } else return 0.0; } @@ -919,17 +1041,29 @@ double CG2Spline::find_closest_point(double x, double y,double max_error,unsigne } } -void CG2Spline::get_part(CG2Spline *spline,double start_length, double end_length) +bool CG2Spline::get_part(CG2Spline *spline,double start_length, double end_length) { + TPoint p; + if(start_length==0.0) spline->set_start_point(this->start); else - spline->set_start_point(this->evaluate(start_length)); - if(end_length==-1.0) + { + if (!this->evaluate(start_length, p)) + return false; + spline->set_start_point(p); + } + + if(end_length==std::numeric_limits<double>::max()) spline->set_end_point(this->end); else - spline->set_end_point(this->evaluate(end_length)); + { + if (!this->evaluate(end_length, p)) + return false; + spline->set_end_point(p); + } spline->generate(this->resolution); + return true; } double CG2Spline::curvature(double u) diff --git a/src/junction.cpp b/src/junction.cpp new file mode 100644 index 0000000000000000000000000000000000000000..bc5c9a24c617822fda61ddaf6f0a54ed4161b201 --- /dev/null +++ b/src/junction.cpp @@ -0,0 +1,743 @@ +#include "junction.h" +#include "exceptions.h" + +CJunction::CJunction() +{ + this->id=-1; + this->resolution=0.1; + this->parent_roadmap=NULL; + this->incomming_roads.clear(); + this->outgoing_roads.clear(); + this->segments.clear(); +} + +CJunction::CJunction(unsigned int id) +{ + this->set_id(id); + this->resolution=0.1; + this->parent_roadmap=NULL; + this->incomming_roads.clear(); + this->outgoing_roads.clear(); + this->segments.clear(); +} + +void CJunction::set_parent_roadmap(CRoadMap *roadmap) +{ + if(roadmap==NULL) + throw CException(_HERE_,"Invalid roadmap reference"); + this->parent_roadmap=roadmap; +} + +void CJunction::set_id(unsigned int id) +{ + this->id=id; +} + +unsigned int CJunction::get_index_by_id(const std::vector<CJunction *> &junctions,unsigned int id) +{ + std::vector<CJunction *>::const_iterator it; + unsigned int index; + + for(it=junctions.begin(),index=0;it!=junctions.end();it++,index++) + if((*it)->id==id) + return index; + + return -1; +} + +CRoadMap &CJunction::get_parent_roadmap(void) +{ + if(this->parent_roadmap==NULL) + throw CException(_HERE_,"No parent road map has been defined"); + return *this->parent_roadmap; +} + +unsigned int CJunction::get_id(void) +{ + return this->id; +} + +void CJunction::set_resolution(double resolution) +{ + this->resolution=resolution; + for(unsigned int i=0;i<this->segments.size();i++) + this->segments[i]->set_resolution(resolution); +} + +double CJunction::get_resolution(double resolution) +{ + return this->resolution; +} + +unsigned int CJunction::get_num_incomming_roads(void) +{ + return this->incomming_roads.size(); +} + +CRoad *CJunction::get_incomming_road_by_index(unsigned int index) +{ + if(index<0 || index >= this->incomming_roads.size()) + throw CException(_HERE_,"Invalid incomming road index"); + return this->incomming_roads[index]; +} + +CRoad &CJunction::get_incomming_road_by_id(unsigned int id) +{ + unsigned int index; + + index=CRoad::get_index_by_id(this->incomming_roads,id); + return *this->get_incomming_road_by_index(index); +} + +unsigned int CJunction::get_incomming_road_index_by_id(unsigned int id) +{ + return CRoad::get_index_by_id(this->incomming_roads,id); +} + +bool CJunction::has_incomming_road(unsigned int id) +{ + if(CRoad::get_index_by_id(this->incomming_roads,id)==(unsigned int)-1) + return false; + else + return true; +} + +unsigned int CJunction::get_num_outgoing_roads(void) +{ + return this->outgoing_roads.size(); +} + +CRoad *CJunction::get_outgoing_road_by_index(unsigned int index) +{ + if(index<0 || index >= this->outgoing_roads.size()) + throw CException(_HERE_,"Invalid outgoing road index"); + return this->outgoing_roads[index]; +} + +unsigned int CJunction::get_outgoing_road_index_by_id(unsigned int id) +{ + return CRoad::get_index_by_id(this->outgoing_roads,id); +} + +CRoad &CJunction::get_outgoing_road_by_id(unsigned int id) +{ + unsigned int index; + + index=CRoad::get_index_by_id(this->outgoing_roads,id); + return *this->get_outgoing_road_by_index(index); +} + +bool CJunction::has_outgoing_road(unsigned int id) +{ + if(CRoad::get_index_by_id(this->outgoing_roads,id)==(unsigned int)-1) + return false; + else + return true; +} + +void CJunction::add_incomming_road(CRoad *road) +{ + unsigned int num_rows; + unsigned int num_cols; + + if(road==NULL) + throw CException(_HERE_,"Invalid incomming road reference"); + if(this->has_incomming_road(road->get_id())) + throw CException(_HERE_,"Incomming road already present"); + if(road->get_num_segments()==0) + throw CException(_HERE_,"Incomming road does not have any geometry"); + if(road->get_num_lanes()==0) + throw CException(_HERE_,"Incomming road does not have any lane"); + this->incomming_roads.push_back(road); + /* add a row to the connectivity matrix */ + if(this->incomming_roads.size()>0 && this->outgoing_roads.size()>0) + { + num_rows=this->incomming_roads.size(); + num_cols=this->outgoing_roads.size(); + this->connectivity.conservativeResize(num_rows,num_cols); + for(unsigned int i=0;i<num_cols;i++) + this->connectivity(num_rows-1,i)=0; + } + /* update road information */ + road->set_next_junction(this); +} + +void CJunction::remove_incomming_road_by_index(unsigned int index) +{ + unsigned int num_rows=this->connectivity.rows(); + unsigned int num_cols=this->connectivity.cols(); + std::vector<CRoad *>::iterator it; + unsigned int i; + + if(index<0 || index >= this->incomming_roads.size()) + throw CException(_HERE_,"Invalid incomming road index"); + for(it=this->incomming_roads.begin(),i=0;it!=this->incomming_roads.end();it++,i++) + { + if(i==index) + { + /* update road information */ + (*it)->set_next_junction(NULL); + this->incomming_roads.erase(it); + break; + } + } + /* remove the corresponding row from the connectivity matrix */ + this->connectivity.block(index,0,num_rows-index-1,num_cols) = this->connectivity.block(index+1,0,num_rows-index-1,num_cols); + this->connectivity.conservativeResize(num_rows-1,num_cols); +} + +void CJunction::remove_incomming_road_by_id(unsigned int id) +{ + unsigned int index; + + index=CRoad::get_index_by_id(this->incomming_roads,id); + this->remove_incomming_road_by_index(index); +} + +void CJunction::add_outgoing_road(CRoad *road) +{ + unsigned int num_rows; + unsigned int num_cols; + + if(road==NULL) + throw CException(_HERE_,"Invalid outgoing road reference"); + if(this->has_outgoing_road(road->get_id())) + throw CException(_HERE_,"Outgoing road already present"); + if(road->get_num_segments()==0) + throw CException(_HERE_,"Outgoing road does not have any geometry"); + if(road->get_num_lanes()==0) + throw CException(_HERE_,"Outgoing road does not have any lane"); + this->outgoing_roads.push_back(road); + /* add a column to the connectivity matrix */ + if(this->incomming_roads.size()>0 && this->outgoing_roads.size()>0) + { + num_rows=this->incomming_roads.size(); + num_cols=this->outgoing_roads.size(); + this->connectivity.conservativeResize(num_rows,num_cols); + for(unsigned int i=0;i<num_rows;i++) + this->connectivity(i,num_cols-1)=0; + } + /* update road information */ + road->set_prev_junction(this); +} + +void CJunction::remove_outgoing_road_by_index(unsigned int index) +{ + unsigned int num_rows=this->connectivity.rows(); + unsigned int num_cols=this->connectivity.cols(); + std::vector<CRoad *>::iterator it; + unsigned int i; + + if(index<0 || index >= this->outgoing_roads.size()) + throw CException(_HERE_,"Invalid outgoing road index"); + for(it=this->outgoing_roads.begin(),i=0;it!=this->outgoing_roads.end();it++,i++) + { + if(i==index) + { + /* update road information */ + (*it)->set_prev_junction(NULL); + this->outgoing_roads.erase(it); + break; + } + } + /* remove the corresponding column from the connectivity matrix */ + this->connectivity.block(0,index,num_rows,num_cols-index-1) = this->connectivity.block(0,index+1,num_rows,num_cols-index-1); + this->connectivity.conservativeResize(num_rows,num_cols-1); +} + +void CJunction::remove_outgoing_road_by_id(unsigned int id) +{ + unsigned int index; + + index=CRoad::get_index_by_id(this->outgoing_roads,id); + this->remove_outgoing_road_by_index(index); +} + +void CJunction::link_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road) +{ + CRoadSegment *new_segment,*in_last_segment,*out_first_segment; + TPoint start_point,end_point; + + if(incomming_road<0 || incomming_road >= this->incomming_roads.size()) + throw CException(_HERE_,"Invalid incomming road index"); + if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size()) + throw CException(_HERE_,"Invalid outgoing road index"); + /* update the connectivity matrix */ + this->connectivity(incomming_road,outgoing_road)=1; + /* create a new road segment */ + in_last_segment=this->incomming_roads[incomming_road]->get_last_segment(); + out_first_segment=this->outgoing_roads[outgoing_road]->get_first_segment(); + in_last_segment->get_end_point(start_point); + out_first_segment->get_start_point(end_point); + new_segment=new CRoadSegment(this->incomming_roads[incomming_road]->get_num_lanes(),this->outgoing_roads[outgoing_road]->get_num_lanes()); + new_segment->set_resolution(this->resolution); + new_segment->set_parent_junction(this); + new_segment->set_geometry(start_point,end_point); + in_last_segment->add_next_segment(new_segment); + new_segment->add_prev_segment(in_last_segment); + new_segment->add_next_segment(out_first_segment); + out_first_segment->add_prev_segment(new_segment); + if(this->parent_roadmap!=NULL) + new_segment->id=this->parent_roadmap->get_next_segment_id(); + this->segments.push_back(new_segment); +} + +void CJunction::link_roads_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + this->link_roads_by_index(incomming_index,outgoing_index); +} + +void CJunction::unlink_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road) +{ + CRoadSegment *in_last_segment,*out_first_segment; + std::vector<CRoadSegment *>::iterator it; + + if(incomming_road<0 || incomming_road >= this->incomming_roads.size()) + throw CException(_HERE_,"Invalid incomming road index"); + if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size()) + throw CException(_HERE_,"Invalid outgoing road index"); + /* update the connectivity matrix */ + this->connectivity(incomming_road,outgoing_road)=0; + /* delete road segment */ + in_last_segment=this->incomming_roads[incomming_road]->get_last_segment(); + out_first_segment=this->outgoing_roads[outgoing_road]->get_first_segment(); + for(it=this->segments.begin();it!=this->segments.end();it++) + { + if((*it)->has_prev_segment(in_last_segment) && (*it)->has_next_segment(out_first_segment)) + { + in_last_segment->remove_next_segment(*it); + out_first_segment->remove_prev_segment(*it); + delete *it; + this->segments.erase(it); + break; + } + } +} + +void CJunction::unlink_roads_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + this->unlink_roads_by_index(incomming_index,outgoing_index); +} + +bool CJunction::are_roads_linked_by_index(unsigned int incomming_road,unsigned int outgoing_road) +{ + if(incomming_road<0 || incomming_road >= this->incomming_roads.size()) + throw CException(_HERE_,"Invalid incomming road index"); + if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size()) + throw CException(_HERE_,"Invalid outgoing road index"); + /* update the connectivity matrix */ + if(this->connectivity(incomming_road,outgoing_road)==1) + return true; + else + return false; +} + +bool CJunction::are_roads_linked_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + return this->are_roads_linked_by_index(incomming_index,outgoing_index); +} + +CRoadSegment *CJunction::link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes) +{ + CRoadSegment *new_segment,*in_last_segment; + TPoint start_point,end_point; + unsigned int incomming_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + if(incomming_index<0 || incomming_index >= this->incomming_roads.size()) + throw CException(_HERE_,"Invalid incomming road index"); + /* connectivity not updated */ + /* create a new road segment */ + in_last_segment=this->incomming_roads[incomming_index]->get_last_segment(); + in_last_segment->get_end_point(start_point); + end_point=outgoing_point; + new_segment=new CRoadSegment(this->incomming_roads[incomming_index]->get_num_lanes(),outgoing_num_lanes); + new_segment->set_resolution(this->resolution); + new_segment->set_parent_junction(this); + new_segment->set_geometry(start_point,end_point); + in_last_segment->add_next_segment(new_segment); + new_segment->add_prev_segment(in_last_segment); + if(this->parent_roadmap!=NULL) + new_segment->id=this->parent_roadmap->get_next_segment_id(); + this->segments.push_back(new_segment); + + return new_segment; +} + +CRoadSegment *CJunction::link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,unsigned int outgoing_road) +{ + CRoadSegment *new_segment,*out_first_segment; + TPoint start_point,end_point; + unsigned int outgoing_index; + + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + if(outgoing_index<0 || outgoing_index >= this->outgoing_roads.size()) + throw CException(_HERE_,"Invalid outgoing road index"); + /* connectivity not updated */ + /* create a new road segment */ + out_first_segment=this->outgoing_roads[outgoing_index]->get_first_segment(); + start_point=incomming_point; + out_first_segment->get_start_point(end_point); + new_segment=new CRoadSegment(incomming_num_lanes,this->outgoing_roads[outgoing_index]->get_num_lanes()); + new_segment->set_resolution(this->resolution); + new_segment->set_parent_junction(this); + new_segment->set_geometry(start_point,end_point); + new_segment->add_next_segment(out_first_segment); + out_first_segment->add_prev_segment(new_segment); + if(this->parent_roadmap!=NULL) + new_segment->id=this->parent_roadmap->get_next_segment_id(); + this->segments.push_back(new_segment); + + return new_segment; +} + +CRoadSegment *CJunction::link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,TPoint &outgoing_point,unsigned int outgoing_num_lanes) +{ + CRoadSegment *new_segment; + TPoint start_point,end_point; + + /* connectivity not updated */ + /* create a new road segment */ + start_point=incomming_point; + end_point=outgoing_point; + new_segment=new CRoadSegment(incomming_num_lanes,outgoing_num_lanes); + new_segment->set_resolution(this->resolution); + new_segment->set_parent_junction(this); + new_segment->set_geometry(start_point,end_point); + if(this->parent_roadmap!=NULL) + new_segment->id=this->parent_roadmap->get_next_segment_id(); + this->segments.push_back(new_segment); + + return new_segment; +} + +void CJunction::link_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out) +{ + CRoadSegment *segment; + + if(incomming_road<0 || incomming_road >= this->incomming_roads.size()) + throw CException(_HERE_,"Invalid incomming road index"); + if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size()) + throw CException(_HERE_,"Invalid outgoing road index"); + if(lane_in>=this->incomming_roads[incomming_road]->get_num_lanes()) + throw CException(_HERE_,"Incomming road does not have that many lanes"); + if(lane_out>=this->outgoing_roads[outgoing_road]->get_num_lanes()) + throw CException(_HERE_,"Outgoing road does not have that many lanes"); + segment=this->get_segment_between_by_index(incomming_road,outgoing_road); + if(segment==NULL) + throw CException(_HERE_,"No segment links the desired roads"); + segment->link_lanes(lane_in,lane_out); +} + +void CJunction::link_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + this->link_lanes_by_index(incomming_index,lane_in,outgoing_index,lane_out); +} + +void CJunction::link_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int num_lanes; + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + + num_lanes=std::min(this->incomming_roads[incomming_index]->get_num_lanes(),this->outgoing_roads[outgoing_index]->get_num_lanes()); + for(unsigned int i=0;i<num_lanes;i++) + this->link_lanes_by_index(incomming_index,i,outgoing_index,i); +} + +void CJunction::link_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + + for(unsigned int i=0;i<this->incomming_roads[incomming_index]->get_num_lanes();i++) + for(unsigned int j=0;j<this->outgoing_roads[outgoing_index]->get_num_lanes();j++) + this->link_lanes_by_index(incomming_index,i,outgoing_index,j); +} + +void CJunction::unlink_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out) +{ + CRoadSegment *segment; + + if(incomming_road<0 || incomming_road >= this->incomming_roads.size()) + throw CException(_HERE_,"Invalid incomming road index"); + if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size()) + throw CException(_HERE_,"Invalid outgoing road index"); + if(lane_in>=this->incomming_roads[incomming_road]->get_num_lanes()) + throw CException(_HERE_,"Incomming road does not have that many lanes"); + if(lane_out>=this->outgoing_roads[outgoing_road]->get_num_lanes()) + throw CException(_HERE_,"Outgoing road does not have that many lanes"); + segment=this->get_segment_between_by_index(incomming_road,outgoing_road); + if(segment==NULL) + throw CException(_HERE_,"No segment links the desired roads"); + segment->unlink_lanes(lane_in,lane_out); +} + +void CJunction::unlink_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + this->unlink_lanes_by_index(incomming_index,lane_in,outgoing_index,lane_out); +} + +void CJunction::unlink_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int num_lanes; + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + + num_lanes=std::min(this->incomming_roads[incomming_index]->get_num_lanes(),this->outgoing_roads[outgoing_index]->get_num_lanes()); + for(unsigned int i=0;i<num_lanes;i++) + this->unlink_lanes_by_index(incomming_index,i,outgoing_index,i); +} + +void CJunction::unlink_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + + for(unsigned int i=0;i<this->incomming_roads[incomming_index]->get_num_lanes();i++) + for(unsigned int j=0;j<this->outgoing_roads[outgoing_index]->get_num_lanes();j++) + this->unlink_lanes_by_index(incomming_index,i,outgoing_index,j); +} + +bool CJunction::are_lanes_linked_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out) +{ + CRoadSegment *segment; + + if(incomming_road<0 || incomming_road >= this->incomming_roads.size()) + throw CException(_HERE_,"Invalid incomming road index"); + if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size()) + throw CException(_HERE_,"Invalid outgoing road index"); + if(lane_in>=this->incomming_roads[incomming_road]->get_num_lanes()) + throw CException(_HERE_,"Incomming road does not have that many lanes"); + if(lane_out>=this->outgoing_roads[outgoing_road]->get_num_lanes()) + throw CException(_HERE_,"Outgoing road does not have that many lanes"); + segment=this->get_segment_between_by_index(incomming_road,outgoing_road); + if(segment==NULL) + throw CException(_HERE_,"No segment links the desired roads"); + return segment->are_lanes_linked(lane_in,lane_out); +} + +bool CJunction::are_lanes_linked_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + return this->are_lanes_linked_by_index(incomming_index,lane_in,outgoing_index,lane_out); +} + +void CJunction::get_connectivity_matrix(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &incomming_id_map,std::vector<unsigned int> &outgoing_id_map) +{ + incomming_id_map.clear(); + for(unsigned int i=0;i<this->incomming_roads.size();i++) + incomming_id_map.push_back(this->incomming_roads[i]->get_id()); + outgoing_id_map.clear(); + for(unsigned int i=0;i<this->outgoing_roads.size();i++) + outgoing_id_map.push_back(this->outgoing_roads[i]->get_id()); + connectivity=this->connectivity; +} + +/* geometry */ +unsigned int CJunction::get_num_segments(void) +{ + return this->segments.size(); +} + +CRoadSegment *CJunction::get_segment_by_index(unsigned int index) +{ + if(index<0 || index >= this->segments.size()) + throw CException(_HERE_,"Invalid segment index"); + return this->segments[index]; +} + +CRoadSegment &CJunction::get_segment_by_id(unsigned int id) +{ + unsigned int index; + + index=CRoadSegment::get_index_by_id(this->segments,id); + return *this->get_segment_by_index(index); +} + +CRoadSegment &CJunction::get_segment_between_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + return *this->get_segment_between_by_index(incomming_index,outgoing_index); +} + +CRoadSegment *CJunction::get_segment_between_by_index(unsigned int incomming_road,unsigned int outgoing_road) +{ + CRoadSegment *in_last_segment,*out_first_segment; + std::vector<CRoadSegment *>::const_iterator it; + + if(incomming_road<0 || incomming_road >= this->incomming_roads.size()) + throw CException(_HERE_,"Invalid incomming road index"); + if(outgoing_road<0 || outgoing_road >= this->outgoing_roads.size()) + throw CException(_HERE_,"Invalid outgoing road index"); + in_last_segment=this->incomming_roads[incomming_road]->get_last_segment(); + out_first_segment=this->outgoing_roads[outgoing_road]->get_first_segment(); + for(it=this->segments.begin();it!=this->segments.end();it++) + { + if((*it)->has_prev_segment(in_last_segment) && (*it)->has_next_segment(out_first_segment)) + { + return *it; + } + } + + return NULL; +} + +bool CJunction::get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol) +{ + double min_offset=std::numeric_limits<double>::max(),tmp_offset,tmp_distance; + TPoint tmp_point; + + for(unsigned int i=0;i<this->segments.size();i++) + { + if(this->segments[i]->get_closest_point(point,tmp_point,tmp_distance,tmp_offset,angle_tol)) + { + if(fabs(tmp_offset)<min_offset) + { + min_offset=fabs(tmp_offset); + distance=tmp_distance; + lateral_offset=tmp_offset; + segment_id=this->segments[i]->get_id(); + } + } + } + if(min_offset!=std::numeric_limits<double>::max()) + return true; + else + return false; +} + +bool CJunction::get_closest_segment_ids(TPoint &point,std::vector<unsigned int >&segment_ids,std::vector<double >&distances,std::vector<double> &lateral_offsets,double angle_tol,double offset_tol) +{ + double tmp_offset,tmp_distance; + TPoint tmp_point; + + segment_ids.clear(); + distances.clear(); + lateral_offsets.clear(); + for(unsigned int i=0;i<this->segments.size();i++) + { + if(this->segments[i]->get_closest_point(point,tmp_point,tmp_distance,tmp_offset,angle_tol)) + { + if(fabs(tmp_offset)<offset_tol) + { + distances.push_back(tmp_distance); + lateral_offsets.push_back(tmp_offset); + segment_ids.push_back(this->segments[i]->get_id()); + } + } + } + if(segment_ids.size()>0) + return true; + else + return false; + +} + +void CJunction::get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw) +{ + std::vector<double> partial_x,partial_y,partial_heading; + + x.clear(); + y.clear(); + yaw.clear(); + for(unsigned int i=0;i<this->segments.size();i++) + { + this->segments[i]->get_geometry(partial_x,partial_y,partial_heading); + x.insert(x.end(),partial_x.begin(),partial_x.end()); + y.insert(y.end(),partial_y.begin(),partial_y.end()); + yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); + } +} + +void CJunction::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw) +{ + std::vector<double> partial_x,partial_y,partial_heading,curvature; + TPoint start_point,end_point; + double lateral_offset_in,lateral_offset_out,width; + CG2Spline *segment_spline; + + x.clear(); + y.clear(); + yaw.clear(); + for(unsigned int i=0;i<this->segments.size();i++) + { + width=this->segments[i]->get_lane_width(); + if(width==0.0) + continue; + + lateral_offset_in=-width/2.0; + for(unsigned int j=0;j<this->segments[i]->get_num_lanes_in();j++) + { + this->segments[i]->get_point_at(0.0,lateral_offset_in,start_point); + lateral_offset_out=-width/2.0; + for(unsigned int k=0;k<this->segments[i]->get_num_lanes_out();k++) + { + this->segments[i]->get_point_at(this->segments[i]->get_length(),lateral_offset_out,end_point); + if(this->segments[i]->are_lanes_linked(j,k)) + { + segment_spline=new CG2Spline(start_point,end_point); + segment_spline->evaluate_all(partial_x,partial_y,curvature,partial_heading); + x.insert(x.end(),partial_x.begin(),partial_x.end()); + y.insert(y.end(),partial_y.begin(),partial_y.end()); + yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); + delete segment_spline; + } + lateral_offset_out-=width; + } + lateral_offset_in-=width; + } + } +} + +CJunction::~CJunction() +{ + this->id=-1; + this->parent_roadmap=NULL; + this->incomming_roads.clear(); + this->outgoing_roads.clear(); + for(unsigned int i=0;i<this->segments.size();i++) + { + if(this->segments[i]!=NULL) + { + delete this->segments[i]; + this->segments[i]=NULL; + } + } + this->segments.clear(); +} + diff --git a/src/opendrive_arc.cpp b/src/opendrive/opendrive_arc.cpp similarity index 65% rename from src/opendrive_arc.cpp rename to src/opendrive/opendrive_arc.cpp index 4d18947e7b54670e93488c37bbb7447aad5ac224..371a49cfea1d104cf67539f84f4cafa9a012da55 100644 --- a/src/opendrive_arc.cpp +++ b/src/opendrive/opendrive_arc.cpp @@ -1,4 +1,5 @@ -#include "opendrive_arc.h" +#include "opendrive/opendrive_arc.h" +#include "common.h" #include <cmath> COpendriveArc::COpendriveArc() @@ -11,13 +12,18 @@ COpendriveArc::COpendriveArc(const COpendriveArc &object) : COpendriveGeometry(o this->curvature=object.curvature; } +COpendriveArc::COpendriveArc(TOpendriveWorldPose &start_pose,double length,double curvature):COpendriveGeometry(start_pose,length) +{ + this->curvature=curvature; +} + bool COpendriveArc::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const { double alpha; - alpha = track.s*this->curvature*this->scale_factor; - local.u = std::sin(alpha)/(this->curvature*this->scale_factor) - track.t*std::sin(alpha); - local.v = (1 - std::cos(alpha))/(this->curvature*this->scale_factor) + track.t*std::cos(alpha); + alpha = track.s*this->curvature; + local.u = std::sin(alpha)/(this->curvature) - track.t*std::sin(alpha); + local.v = (1 - std::cos(alpha))/(this->curvature) + track.t*std::cos(alpha); local.heading = normalize_angle(track.heading + alpha); return true; } @@ -41,6 +47,14 @@ void COpendriveArc::load_params(const planView::geometry_type &geometry_info) this->curvature = (geometry_info.arc().get().curvature().present() ? geometry_info.arc().get().curvature().get() : 0.0); } +void COpendriveArc::save_params(planView::geometry_type &geometry_info) +{ + ::arc arc; + + arc.curvature(this->curvature); + geometry_info.arc(arc); +} + COpendriveGeometry *COpendriveArc::clone(void) { COpendriveArc *new_arc=new COpendriveArc(*this); @@ -50,8 +64,13 @@ COpendriveGeometry *COpendriveArc::clone(void) void COpendriveArc::get_curvature(double &start,double &end) { - start=this->curvature*this->scale_factor; - end=this->curvature*this->scale_factor; + start=this->curvature; + end=this->curvature; +} + +void COpendriveArc::get_curvature_at(double length,double &curvature) +{ + curvature=this->curvature; } void COpendriveArc::operator=(const COpendriveArc &object) diff --git a/src/opendrive_geometry.cpp b/src/opendrive/opendrive_geometry.cpp similarity index 74% rename from src/opendrive_geometry.cpp rename to src/opendrive/opendrive_geometry.cpp index f3380799a6ecf86d299f20045e4a54e5d2cb0c1b..4f7357c9e8f2e22f722a37464e740fa9b2e46241 100644 --- a/src/opendrive_geometry.cpp +++ b/src/opendrive/opendrive_geometry.cpp @@ -1,4 +1,5 @@ -#include "opendrive_geometry.h" +#include "opendrive/opendrive_geometry.h" +#include "common.h" #include <cmath> COpendriveGeometry::COpendriveGeometry() @@ -8,7 +9,6 @@ COpendriveGeometry::COpendriveGeometry() this->pose.x = 0.0; this->pose.y = 0.0; this->pose.heading = 0.0; - this->scale_factor=DEFAULT_SCALE_FACTOR; } COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object) @@ -18,7 +18,13 @@ COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object) this->pose.x = object.pose.x; this->pose.y = object.pose.y; this->pose.heading = object.pose.heading; - this->scale_factor = object.scale_factor; +} + +COpendriveGeometry::COpendriveGeometry(TOpendriveWorldPose &start_pose,double length) +{ + this->min_s=0.0; + this->set_max_s(length); + this->set_start_pose(start_pose); } void COpendriveGeometry::print(std::ostream& out) @@ -40,15 +46,20 @@ void COpendriveGeometry::load(const planView::geometry_type &geometry_info) this->load_params(geometry_info); } -void COpendriveGeometry::set_scale_factor(double scale) +void COpendriveGeometry::save(planView::geometry_type &geometry_info) { - this->scale_factor=scale; + geometry_info.s(this->min_s); + geometry_info.x(this->pose.x); + geometry_info.y(this->pose.y); + geometry_info.hdg(this->pose.heading); + geometry_info.length(this->max_s); + this->save_params(geometry_info); } void COpendriveGeometry::set_start_pose(TOpendriveWorldPose &pose) { - this->pose.x=pose.x*this->scale_factor; - this->pose.y=pose.y*this->scale_factor; + this->pose.x=pose.x; + this->pose.y=pose.y; this->pose.heading=pose.heading; } @@ -57,7 +68,7 @@ void COpendriveGeometry::set_max_s(double s) if(s<this->min_s) this->max_s=this->min_s; else - this->max_s=s*this->scale_factor; + this->max_s=s; } void COpendriveGeometry::set_min_s(double s) @@ -65,7 +76,7 @@ void COpendriveGeometry::set_min_s(double s) if(s>this->max_s) this->min_s=this->max_s; else - this->min_s=s*this->scale_factor; + this->min_s=s; } bool COpendriveGeometry::get_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const @@ -80,8 +91,8 @@ bool COpendriveGeometry::get_world_pose(const TOpendriveTrackPose &track,TOpendr if(this->transform_local_pose(track,local)) { world.heading=normalize_angle(this->pose.heading+local.heading); - 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; + world.x=local.u*std::cos(this->pose.heading)-local.v*std::sin(this->pose.heading)+this->pose.x; + world.y=local.u*std::sin(this->pose.heading)+local.v*std::cos(this->pose.heading)+this->pose.y; return true; } else @@ -90,7 +101,7 @@ bool COpendriveGeometry::get_world_pose(const TOpendriveTrackPose &track,TOpendr bool COpendriveGeometry::in_range(double s) const { - if((s<(this->max_s/this->scale_factor)) && (s>=(this->min_s/this->scale_factor))) + if((s<this->max_s) && (s>=this->min_s)) return true; else return false; @@ -98,25 +109,25 @@ bool COpendriveGeometry::in_range(double s) const double COpendriveGeometry::get_length(void) const { - return (this->max_s-this->min_s)/this->scale_factor; + return (this->max_s-this->min_s); } double COpendriveGeometry::get_max_s(void) const { - return this->max_s/this->scale_factor; + return this->max_s; } double COpendriveGeometry::get_min_s(void) const { - return this->min_s/this->scale_factor; + return this->min_s; } TOpendriveWorldPose COpendriveGeometry::get_start_pose(void) const { TOpendriveWorldPose tmp_pose; - tmp_pose.x=this->pose.x/this->scale_factor; - tmp_pose.y=this->pose.y/this->scale_factor; + tmp_pose.x=this->pose.x; + tmp_pose.y=this->pose.y; tmp_pose.heading=this->pose.heading; return tmp_pose; @@ -135,6 +146,22 @@ TOpendriveWorldPose COpendriveGeometry::get_end_pose(void) const return world_pose; } +bool COpendriveGeometry::get_pose_at(double length,TOpendriveWorldPose &pose) const +{ + TOpendriveTrackPose track_pose; + + if(length >= 0.0 && length <= this->get_length()) + { + track_pose.s=length; + track_pose.t=0.0; + track_pose.heading=0.0; + this->get_world_pose(track_pose,pose); + return true; + } + else + return false; +} + void COpendriveGeometry::operator=(const COpendriveGeometry &object) { this->min_s = object.min_s; @@ -142,7 +169,6 @@ void COpendriveGeometry::operator=(const COpendriveGeometry &object) this->pose.x = object.pose.x; this->pose.y = object.pose.y; this->pose.heading = object.pose.heading; - this->scale_factor = object.scale_factor; } std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom) @@ -158,6 +184,5 @@ COpendriveGeometry::~COpendriveGeometry() this->pose.x = 0.0; this->pose.y = 0.0; this->pose.heading = 0.0; - this->scale_factor = DEFAULT_SCALE_FACTOR; } diff --git a/src/opendrive/opendrive_junction.cpp b/src/opendrive/opendrive_junction.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9651b7ffb977807990e060ea354c76b475b0ba53 --- /dev/null +++ b/src/opendrive/opendrive_junction.cpp @@ -0,0 +1,347 @@ +#include "opendrive/opendrive_junction.h" +#include "opendrive/opendrive_param_poly3.h" + +#include "exceptions.h" + +#include <sstream> + +COpendriveJunction::COpendriveJunction() +{ + this->id=-1; + this->name=""; + this->connections.clear(); +} + +void COpendriveJunction::set_name(const std::string &name) +{ + this->name=name; +} + +void COpendriveJunction::set_id(unsigned int id) +{ + this->id=id; +} + +void COpendriveJunction::load(OpenDRIVE::junction_type &junction_info) +{ + std::stringstream error; + TJunctionConnection new_connection; + TJunctionLink new_link; + + this->set_name(junction_info.name().get()); + this->set_id(std::stol(junction_info.id().get())); + for(junction::connection_iterator connection_it(junction_info.connection().begin()); connection_it!=junction_info.connection().end();++connection_it) + { + new_connection.id=std::stol(connection_it->id().get()); + if(connection_it->incomingRoad().present()) + new_connection.incomming_road_id=std::stol(connection_it->incomingRoad().get()); + else + { + error << "Connectivity information missing for junction " << this->name << ": No incomming segment"; + throw CException(_HERE_,error.str()); + } + if(connection_it->connectingRoad().present()) + new_connection.connecting_road_id=std::stol(connection_it->connectingRoad().get()); + else + { + error << "Connectivity information missing for junction " << this->name << ": No connecting segment"; + throw CException(_HERE_,error.str()); + } + if(connection_it->contactPoint().present()) + { + if(connection_it->contactPoint().get().compare("end")==0) + new_connection.end_contact_point=true; + else + new_connection.end_contact_point=false; + } + else + { + error << "Connectivity information missing for junction " << this->name << ": No contact point"; + throw CException(_HERE_,error.str()); + } + new_connection.links.clear(); + for(connection::laneLink_iterator lane_link_it(connection_it->laneLink().begin()); lane_link_it!=connection_it->laneLink().end();++lane_link_it) + { + if(lane_link_it->from().present()) + new_link.from_lane_id=lane_link_it->from().get(); + else + { + error << "Connectivity information missing for junction " << this->name << ": lane link missing from identifier"; + throw CException(_HERE_,error.str()); + } + if(lane_link_it->to().present()) + new_link.to_lane_id=lane_link_it->to().get(); + else + { + error << "Connectivity information missing for junction " << this->name << ": lane link missing to identifier"; + throw CException(_HERE_,error.str()); + } + new_connection.links.push_back(new_link); + } + this->connections.push_back(new_connection); + } +} + +void COpendriveJunction::save(OpenDRIVE::junction_type &junction_info) +{ + junction_info.id(std::to_string(this->id)); + junction_info.name(this->name); + for(unsigned int i=0;i<this->connections.size();i++) + { + ::connection new_connection; + new_connection.id(std::to_string(this->connections[i].id)); + new_connection.incomingRoad(std::to_string(this->connections[i].incomming_road_id)); + new_connection.connectingRoad(std::to_string(this->connections[i].connecting_road_id)); + if(this->connections[i].end_contact_point) + new_connection.contactPoint("end"); + else + new_connection.contactPoint("start"); + for(unsigned int j=0;j<this->connections[i].links.size();j++) + { + ::laneLink new_link; + new_link.from(this->connections[i].links[j].from_lane_id); + new_link.to(this->connections[i].links[j].to_lane_id); + new_connection.laneLink().push_back(new_link); + } + junction_info.connection().push_back(new_connection); + } +} + +void COpendriveJunction::convert(CJunction **junction,opendrive_road_map_t &road_map) +{ + CRoad *in=NULL,*out=NULL; + unsigned int from_index,to_index; + + (*junction)=new CJunction(); + + // add incomming and outgoing roads + for(unsigned int i=0;i<this->connections.size();i++) + { + for(opendrive_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++) + { + if(it->first->get_id()==this->connections[i].incomming_road_id) + { + if(it->first->has_successor() && it->first->is_successor_junction() && it->first->get_successor_id()==this->id) + { + if((it->second).size()>0) + { + in=(it->second)[0]; + if(!(*junction)->has_incomming_road(in->get_id())) + (*junction)->add_incomming_road(in); + } + } + else if(it->first->has_predecessor() && it->first->is_predecessor_junction() && it->first->get_predecessor_id()==this->id) + { + if((it->second).size()>1) + { + in=(it->second)[1]; + if(!(*junction)->has_incomming_road(in->get_id())) + (*junction)->add_incomming_road(in); + } + } + + } + if(it->first->get_id()==this->connections[i].connecting_road_id) + { + if(it->first->has_predecessor() && it->first->is_predecessor_junction() && it->first->get_predecessor_id()==this->id) + { + if((it->second).size()>0) + { + out=(it->second)[0]; + if(!(*junction)->has_outgoing_road(out->get_id())) + (*junction)->add_outgoing_road(out); + } + } + else if(it->first->has_successor() && it->first->is_successor_junction() && it->first->get_successor_id()==this->id) + { + if((it->second).size()>1) + { + out=(it->second)[1]; + if(!(*junction)->has_outgoing_road(out->get_id())) + (*junction)->add_outgoing_road(out); + } + } + } + } + if(in!=NULL && out!=NULL) + (*junction)->link_roads_by_id(in->get_id(),out->get_id()); + /* craete the connectivity */ + for(unsigned int j=0;j<this->connections[i].links.size();j++) + { + from_index=abs(this->connections[i].links[j].from_lane_id)-1; + to_index=abs(this->connections[i].links[j].to_lane_id)-1; + (*junction)->link_lanes_by_id(in->get_id(),from_index,out->get_id(),to_index); + } + } +} + +bool COpendriveJunction::get_road_data(unsigned int id,opendrive_road_inverse_map_t &road_map,COpendriveRoadSegment **road,bool incomming) +{ + for(unsigned int i=0;i<road_map.size();i++) + { + if(road_map[i].first.size()>0) + { + if(road_map[i].first[0]->get_id()==id) + { + if(incomming) + (*road)=road_map[i].second[road_map[i].second.size()-1]; + else + (*road)=road_map[i].second[0]; + return true; + } + } + if(road_map[i].first.size()>1) + { + if(road_map[i].first[1]->get_id()==id) + { + if(incomming) + (*road)=road_map[i].second[0]; + else + (*road)=road_map[i].second[road_map[i].second.size()-1]; + return false; + } + } + } + throw CException(_HERE_,"No road data for the given ID"); +} + +void COpendriveJunction::generate_road_segment(CRoadSegment *segment,unsigned int ¤t_id,unsigned int road_in,int lane_in,unsigned int road_out,int lane_out,COpendriveRoadSegment *new_segment) +{ + std::stringstream txt; + double lateral_offset_in,lateral_offset_out; + TPoint start_point,end_point; + TOpendriveWorldPose start_pose,end_pose; + COpendriveParamPoly3 *new_geometry; + COpendriveLane *new_lane; + + txt << "junction_" << road_in << "_" << lane_in << "_" << road_out << "_" << lane_out; + new_segment->set_name(txt.str()); + new_segment->set_id(current_id); + current_id++; + new_segment->set_center_mark(OD_MARK_NONE); + new_segment->set_junction_id(segment->get_parent_junction().get_id()); + // add geometry + lateral_offset_in=-(abs(lane_in)-1)*segment->get_lane_width(); + segment->get_point_at(0.0,lateral_offset_in,start_point); + start_pose.x=start_point.x; + start_pose.y=start_point.y; + start_pose.heading=start_point.heading; + lateral_offset_out=-(abs(lane_out)-1)*segment->get_lane_width(); + segment->get_point_at(segment->get_length(),lateral_offset_out,end_point); + end_pose.x=end_point.x; + end_pose.y=end_point.y; + end_pose.heading=end_point.heading; + new_geometry=new COpendriveParamPoly3(start_pose,end_pose); + new_segment->add_geometry(new_geometry,segment->get_resolution()); + // add a single right lane + new_lane=new COpendriveLane(); + new_lane->set_speed(segment->get_lane_speed()); + new_lane->set_width(segment->get_lane_width()); + new_lane->set_id(-1); + new_lane->set_right_mark(OD_MARK_NONE); + new_segment->add_right_lane(new_lane); +} + +void COpendriveJunction::convert(CJunction *junction_in,unsigned int ¤t_id,COpendriveJunction *junction_out,std::vector<COpendriveRoadSegment *> &segments,opendrive_road_inverse_map_t &road_map) +{ + std::stringstream txt; + CRoadSegment *segment; + COpendriveRoadSegment *incomming_segment,*outgoing_segment,*new_segment; + TJunctionConnection new_connection; + TJunctionLink new_link; + bool incomming_right,outgoing_right; + int incomming_lane,outgoing_lane; + unsigned int ids=0,incomming_id,outgoing_id; + + if(junction_in==NULL) + throw CException(_HERE_,"Invalid junction reference"); + segments.clear(); + txt << "junction" << junction_in->get_id(); + junction_out->set_name(txt.str()); + junction_out->set_id(junction_in->get_id()); + for(unsigned int i=0;i<junction_in->get_num_incomming_roads();i++) + { + // get incomming opendrive road info + incomming_id=junction_in->get_incomming_road_by_index(i)->get_id(); + incomming_right=COpendriveJunction::get_road_data(incomming_id,road_map,&incomming_segment,true); + new_connection.incomming_road_id=incomming_segment->get_id(); + new_connection.end_contact_point=false; + for(unsigned int j=0;j<junction_in->get_num_outgoing_roads();j++) + { + outgoing_id=junction_in->get_outgoing_road_by_index(j)->get_id(); + outgoing_right=COpendriveJunction::get_road_data(outgoing_id,road_map,&outgoing_segment,false); + new_connection.connecting_road_id=outgoing_segment->get_id(); + new_connection.links.clear(); + if(junction_in->are_roads_linked_by_id(incomming_id,outgoing_id)) + { + new_connection.id=ids; + ids++; + segment=&junction_in->get_segment_between_by_id(incomming_id,outgoing_id); + // create road from segment + for(unsigned int k=0;k<segment->get_num_lanes_in();k++) + { + if(incomming_right) + incomming_lane=-(k+1); + else + incomming_lane=k+1; + for(unsigned int l=0;l<segment->get_num_lanes_out();l++) + { + if(outgoing_right) + outgoing_lane=-(k+1); + else + outgoing_lane=k+1; + if(segment->are_lanes_linked(k,l)) + { + new_link.from_lane_id=incomming_lane; + new_link.to_lane_id=outgoing_lane; + new_connection.links.push_back(new_link); + // create links + new_segment=new COpendriveRoadSegment(); + COpendriveJunction::generate_road_segment(segment,current_id,incomming_segment->get_id(),incomming_lane,outgoing_segment->get_id(),outgoing_lane,new_segment); + segments.push_back(new_segment); + } + } + } + if(new_connection.links.size()>0) + junction_out->connections.push_back(new_connection); + } + } + } +} + +std::string COpendriveJunction::get_name(void) const +{ + return this->name; +} + +unsigned int COpendriveJunction::get_id(void) const +{ + return this->id; +} + +unsigned int COpendriveJunction::get_num_connections(void) const +{ + return this->connections.size(); +} + +const TJunctionConnection &COpendriveJunction::get_connection(unsigned int index) const +{ + std::stringstream error; + + if(index>=0 && index<this->connections.size()) + return this->connections[index]; + else + { + error << "Invalid connection index " << index; + throw CException(_HERE_,error.str()); + } +} + +COpendriveJunction::~COpendriveJunction() +{ + this->connections.clear(); + this->name=""; + this->id=-1; +} + + diff --git a/src/opendrive/opendrive_lane.cpp b/src/opendrive/opendrive_lane.cpp new file mode 100644 index 0000000000000000000000000000000000000000..733f44909f3a49aea7817d46d3be041bc9064e0e --- /dev/null +++ b/src/opendrive/opendrive_lane.cpp @@ -0,0 +1,176 @@ +#include "opendrive/opendrive_lane.h" +#include "exceptions.h" +#include <math.h> + +COpendriveLane::COpendriveLane() +{ + this->right_mark=OD_MARK_NONE; + this->width=0.0; + this->speed=0.0; + this->id=0; +} + +void COpendriveLane::set_right_mark(opendrive_mark_t mark) +{ + this->right_mark=mark; +} + +void COpendriveLane::set_width(double width) +{ + this->width=width; +} + +void COpendriveLane::set_speed(double speed) +{ + this->speed=speed; +} + +void COpendriveLane::set_id(int id) +{ + this->id=id; +} + +void COpendriveLane::load(const ::lane &lane_info) +{ + std::stringstream error; + opendrive_mark_t mark; + + this->right_mark=OD_MARK_NONE; + this->set_id(lane_info.id().get()); + // import lane width + if(lane_info.width().size()<1) + { + error << "Lane " << this->id << " has no width record"; + throw CException(_HERE_,error.str()); + } + else if(lane_info.width().size()>1) + { + error << "Lane " << this->id << " has more than one width record"; + throw CException(_HERE_,error.str()); + } + this->set_width(lane_info.width().begin()->a().get()); + // import lane speed + if(lane_info.speed().size()<1) + this->set_speed(60.0); + else if(lane_info.speed().size()>1) + { + error << "Lane " << this->id << " has more than one speed record"; + throw CException(_HERE_,error.str()); + } + else + this->set_speed(lane_info.speed().begin()->max().get()); + //lane mark + if(lane_info.roadMark().size()==0) + mark=OD_MARK_NONE; + else if(lane_info.roadMark().size()>1) + { + error << "Lane " << this->id << " has more than one road mark record"; + throw CException(_HERE_,error.str()); + } + else if(lane_info.roadMark().size()==1) + { + 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; + else if(lane_info.roadMark().begin()->type1().get()=="solid solid") + mark=OD_MARK_SOLID_SOLID; + else if(lane_info.roadMark().begin()->type1().get()=="solid broken") + mark=OD_MARK_SOLID_BROKEN; + else if(lane_info.roadMark().begin()->type1().get()=="broken solid") + mark=OD_MARK_BROKEN_SOLID; + else if(lane_info.roadMark().begin()->type1().get()=="broken broken") + mark=OD_MARK_BROKEN_BROKEN; + else + mark=OD_MARK_NONE; + } + else + mark=OD_MARK_NONE; + } + this->right_mark=mark; +} + +void COpendriveLane::save(::lane &lane_info) +{ + ::width lane_width; + ::speed lane_speed; + ::roadMark lane_mark; + + lane_info.id(this->id); + lane_info.type("driving"); + lane_info.level("false"); + lane_width.sOffset(0.0); + lane_width.a(this->width); + lane_width.b(0.0); + lane_width.c(0.0); + lane_width.d(0.0); + lane_info.width().push_back(lane_width); + lane_speed.sOffset(0.0); + lane_speed.max(this->speed); + lane_info.speed().push_back(lane_speed); + switch(this->right_mark) + { + case OD_MARK_NONE: + lane_mark.type1("none"); + break; + case OD_MARK_SOLID: + lane_mark.type1("solid"); + break; + case OD_MARK_BROKEN: + lane_mark.type1("broken"); + break; + case OD_MARK_SOLID_SOLID: + lane_mark.type1("solid solid"); + break; + case OD_MARK_SOLID_BROKEN: + lane_mark.type1("solid broken"); + break; + case OD_MARK_BROKEN_SOLID: + lane_mark.type1("broken solid"); + break; + case OD_MARK_BROKEN_BROKEN: + lane_mark.type1("broken broken"); + break; + default: + lane_mark.type1("none"); + break; + } + lane_mark.sOffset(0.0); + lane_mark.weight("standard"); + lane_mark.color("standard"); + lane_mark.width(0.3); + lane_mark.laneChange("both"); + lane_info.roadMark().push_back(lane_mark); +} + +opendrive_mark_t COpendriveLane::get_right_road_mark(void) const +{ + return this->right_mark; +} + +double COpendriveLane::get_width(void) const +{ + return this->width; +} + +double COpendriveLane::get_speed(void) const +{ + return this->speed; +} + +int COpendriveLane::get_id(void) const +{ + return this->id; +} + +COpendriveLane::~COpendriveLane() +{ + this->right_mark=OD_MARK_NONE; + this->width=0.0; + this->speed=0.0; + this->id=0; +} diff --git a/src/opendrive_line.cpp b/src/opendrive/opendrive_line.cpp similarity index 71% rename from src/opendrive_line.cpp rename to src/opendrive/opendrive_line.cpp index 9b0fa5eb1cb0b722d8bd3f45651cb25f760325ed..469a97a8f936cee5e2c8bfc28b7bfa95064f69dd 100644 --- a/src/opendrive_line.cpp +++ b/src/opendrive/opendrive_line.cpp @@ -1,4 +1,4 @@ -#include "opendrive_line.h" +#include "opendrive/opendrive_line.h" COpendriveLine::COpendriveLine() { @@ -10,6 +10,11 @@ COpendriveLine::COpendriveLine(const COpendriveGeometry &object) : COpendriveGeo } +COpendriveLine::COpendriveLine(TOpendriveWorldPose &start_pose,double length):COpendriveGeometry(start_pose,length) +{ + +} + bool COpendriveLine::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const { local.u=track.s; @@ -34,6 +39,13 @@ void COpendriveLine::load_params(const planView::geometry_type &geometry_info) } +void COpendriveLine::save_params(planView::geometry_type &geometry_info) +{ + ::line1 new_line; + + geometry_info.line(new_line); +} + COpendriveGeometry *COpendriveLine::clone(void) { COpendriveLine *new_line=new COpendriveLine(*this); @@ -47,6 +59,11 @@ void COpendriveLine::get_curvature(double &start,double &end) end=0.0; } +void COpendriveLine::get_curvature_at(double length,double &curvature) +{ + curvature=0.0; +} + void COpendriveLine::operator=(const COpendriveLine &object) { COpendriveGeometry::operator=(object); diff --git a/src/opendrive_object.cpp b/src/opendrive/opendrive_object.cpp similarity index 82% rename from src/opendrive_object.cpp rename to src/opendrive/opendrive_object.cpp index 580d41f8df3471bf34a6d82182179ce75aec981e..75fe964e2ee1c6a2768189de7e2adaf76ac5bc35 100644 --- a/src/opendrive_object.cpp +++ b/src/opendrive/opendrive_object.cpp @@ -1,22 +1,19 @@ -#include "opendrive_object.h" +#include "opendrive/opendrive_object.h" #include "exceptions.h" #include <cmath> COpendriveObject::COpendriveObject() { - this->segment=NULL; this->pose.s=0.0; this->pose.t=0.0; this->pose.heading=0.0; this->type=""; this->name=""; this->object_type=OD_NONE; - this->scale_factor=DEFAULT_SCALE_FACTOR; } COpendriveObject::COpendriveObject(const COpendriveObject& object) { - this->segment=object.segment; this->pose.s=object.pose.s; this->pose.s=object.pose.t; this->pose.heading=object.pose.heading; @@ -45,10 +42,9 @@ COpendriveObject::COpendriveObject(const COpendriveObject& object) case OD_NONE: break; } - this->scale_factor=object.scale_factor; } -void COpendriveObject::load(objects::object_type &object_info,COpendriveRoadSegment *segment) +void COpendriveObject::load(objects::object_type &object_info) { double u,v; unsigned int i; @@ -105,49 +101,19 @@ void COpendriveObject::load(objects::object_type &object_info,COpendriveRoadSegm } else this->object_type=OD_NONE; - this->segment=segment; -} - -void COpendriveObject::update_references(segment_up_ref_t &segment_refs) -{ - if(segment_refs.find(this->segment)!=segment_refs.end()) - this->segment=segment_refs[this->segment]; -} - -void COpendriveObject::set_scale_factor(double scale) -{ - this->scale_factor=scale; -} - -double COpendriveObject::get_scale_factor(void) -{ - return this->scale_factor; } TOpendriveTrackPose COpendriveObject::get_track_pose(void) const { TOpendriveTrackPose track; - track.s=this->pose.s/this->scale_factor; - track.t=this->pose.t/this->scale_factor; + track.s=this->pose.s; + track.t=this->pose.t; track.heading=this->pose.heading; return track; } -TOpendriveWorldPose COpendriveObject::get_world_pose(void) const -{ - TOpendriveWorldPose world; - - if(this->segment!=NULL) - { - world=segment->transform_to_world_pose(this->get_track_pose()); - return world; - } - else - throw CException(_HERE_,"Invalid parent segment reference"); -} - int COpendriveObject::get_id(void) const { return this->id; @@ -199,9 +165,9 @@ TOpendriveBox COpendriveObject::get_box_data(void) const { TOpendriveBox data; - data.length=this->object.box.length/this->scale_factor; - data.width=this->object.box.width/this->scale_factor; - data.height=this->object.box.height/this->scale_factor; + data.length=this->object.box.length; + data.width=this->object.box.width; + data.height=this->object.box.height; return data; } @@ -210,8 +176,8 @@ TOpendriveCylinder COpendriveObject::get_cylinder_data(void) const { TOpendriveCylinder data; - data.radius=this->object.cylinder.radius/this->scale_factor; - data.height=this->object.cylinder.height/this->scale_factor; + data.radius=this->object.cylinder.radius; + data.height=this->object.cylinder.height; return data; } @@ -222,9 +188,9 @@ TOpendrivePolygon COpendriveObject::get_polygon_data(void) const for(unsigned int i=0;i<this->object.polygon.num_vertices;i++) { - data.height[i]=this->object.polygon.height[i]/this->scale_factor; - data.vertices[i].s=this->object.polygon.vertices[i].s/this->scale_factor; - data.vertices[i].t=this->object.polygon.vertices[i].t/this->scale_factor; + data.height[i]=this->object.polygon.height[i]; + data.vertices[i].s=this->object.polygon.vertices[i].s; + data.vertices[i].t=this->object.polygon.vertices[i].t; data.vertices[i].heading=this->object.polygon.vertices[i].heading; } return data; diff --git a/src/opendrive_param_poly3.cpp b/src/opendrive/opendrive_param_poly3.cpp similarity index 53% rename from src/opendrive_param_poly3.cpp rename to src/opendrive/opendrive_param_poly3.cpp index e57b486a43ab74db4fafcf64902e4e8eaded21f7..bbb3354bb1a7423782396212724aaaf09ec430a7 100644 --- a/src/opendrive_param_poly3.cpp +++ b/src/opendrive/opendrive_param_poly3.cpp @@ -1,6 +1,9 @@ -#include "opendrive_param_poly3.h" +#include "opendrive/opendrive_param_poly3.h" +#include "common.h" #include <cmath> +#include <Eigen/Dense> + COpendriveParamPoly3::COpendriveParamPoly3() { this->u.a=0.0; @@ -27,14 +30,84 @@ COpendriveParamPoly3::COpendriveParamPoly3(const COpendriveParamPoly3 &object) : this->normalized=object.normalized; } +COpendriveParamPoly3::COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose) +{ + TOpendriveWorldPose end_pose_zero,end_pose_rot; + Eigen::MatrixXd A=Eigen::MatrixXd::Zero(6,8),pinv; + Eigen::VectorXd b=Eigen::VectorXd::Zero(6,1),sol; + double length=0.0,x,prev_x,y,prev_y,p,p2,p3; + + end_pose_zero.x=end_pose.x-start_pose.x; + end_pose_zero.y=end_pose.y-start_pose.y; + end_pose_zero.heading=end_pose.heading; + end_pose_rot.x=end_pose_zero.x*std::cos(start_pose.heading)+end_pose_zero.y*std::sin(start_pose.heading); + end_pose_rot.y=-end_pose_zero.x*std::sin(start_pose.heading)+end_pose_zero.y*std::cos(start_pose.heading); + end_pose_rot.heading=end_pose.heading-start_pose.heading; + A(0,0)=1.0; + A(1,4)=1.0; + A(2,0)=1.0; + A(2,1)=1.0; + A(2,2)=1.0; + A(2,3)=1.0; + A(3,4)=1.0; + A(3,5)=1.0; + A(3,6)=1.0; + A(3,7)=1.0; + A(4,1)=-tan(0.0); + A(4,5)=1.0; + A(5,1)=-tan(end_pose_rot.heading); + A(5,2)=-2.0*tan(end_pose_rot.heading); + A(5,3)=-3.0*tan(end_pose_rot.heading); + A(5,5)=1.0; + A(5,6)=2.0; + A(5,7)=3.0; + pinv=A.completeOrthogonalDecomposition().pseudoInverse(); + b(0)=0.0; + b(1)=0.0; + b(2)=end_pose_rot.x; + b(3)=end_pose_rot.y; + sol=pinv*b; + this->u.a=sol(0); + this->u.b=sol(1); + this->u.c=sol(2); + this->u.d=sol(3); + this->v.a=sol(4); + this->v.b=sol(5); + this->v.c=sol(6); + this->v.d=sol(7); + + this->normalized=true; + // compute start pose + COpendriveGeometry::set_start_pose(start_pose); + // compute length + prev_x=this->u.a; + prev_y=this->v.a; + for(unsigned int i=1;i<=100;i++) + { + p=i*0.01; + p2=p*p; + p3=p2*p; + x=this->u.a + this->u.b*p + this->u.c*p2 + this->u.d*p3; + y=this->v.a + this->v.b*p + this->v.c*p2 + this->v.d*p3; + length+=sqrt(pow(x-prev_x,2.0)+pow(y-prev_y,2.0)); + prev_x=x; + prev_y=y; + } + this->set_max_s(length); +} + bool COpendriveParamPoly3::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const { - double p = (this->normalized ? track.s/((this->max_s - this->min_s)/this->scale_factor):track.s); + double p = (this->normalized ? track.s/(this->max_s - this->min_s):track.s); double p2 = p*p; double p3 = p2*p; double du = this->u.b + 2*this->u.c*p + 3*this->u.d*p2; double dv = this->v.b + 2*this->v.c*p + 3*this->v.d*p2; - double alpha = std::atan2(dv, du); + double alpha; + if(p==0.0) + alpha = 0.0; + else + alpha = std::atan2(dv,du); local.u = this->u.a + this->u.b*p + this->u.c*p2 + this->u.d*p3 - track.t*std::sin(alpha); local.v = this->v.a + this->v.b*p + this->v.c*p2 + this->v.d*p3 + track.t*std::cos(alpha); local.heading = normalize_angle(track.heading + alpha); @@ -73,6 +146,55 @@ void COpendriveParamPoly3::load_params(const planView::geometry_type &geometry_i this->normalized = false; } +void COpendriveParamPoly3::save_params(planView::geometry_type &geometry_info) +{ + ::paramPoly3 poly; + + poly.aU(this->u.a); + poly.bU(this->u.b); + poly.cU(this->u.c); + poly.dU(this->u.d); + poly.aV(this->v.a); + poly.bV(this->v.b); + poly.cV(this->v.c); + poly.dV(this->v.d); + if(this->normalized) + poly.pRange(pRange::normalized); + else + poly.pRange(pRange::arcLength); + geometry_info.paramPoly3(poly); +} + +COpendriveParamPoly3::COpendriveParamPoly3(TOpendriveWorldPose &start_pose,TOpendrivePoly3Params &u_params,TOpendrivePoly3Params &v_params) +{ + double length=0.0,x,prev_x=u_params.a,y,prev_y=v_params.a,p,p2,p3; + + this->u.a=u_params.a; + this->u.b=u_params.b; + this->u.c=u_params.c; + this->u.d=u_params.d; + this->v.a=v_params.a; + this->v.b=v_params.b; + this->v.c=v_params.c; + this->v.d=v_params.d; + this->normalized=true; + // compute start pose + COpendriveGeometry::set_start_pose(start_pose); + // compute length + for(unsigned int i=1;i<=100;i++) + { + p=i*0.01; + p2=p*p; + p3=p2*p; + x=this->u.a + this->u.b*p + this->u.c*p2 + this->u.d*p3; + y=this->v.a + this->v.b*p + this->v.c*p2 + this->v.d*p3; + length+=sqrt(pow(x-prev_x,2.0)+pow(y-prev_y,2.0)); + prev_x=x; + prev_y=y; + } + this->set_max_s(length); +} + COpendriveGeometry *COpendriveParamPoly3::clone(void) { COpendriveParamPoly3 *new_poly=new COpendriveParamPoly3(*this); @@ -86,6 +208,11 @@ void COpendriveParamPoly3::get_curvature(double &start,double &end) end=0.0; } +void COpendriveParamPoly3::get_curvature_at(double length,double &curvature) +{ + curvature=0.0; +} + TOpendrivePoly3Params COpendriveParamPoly3::get_u_params(void) { return this->u; diff --git a/src/opendrive/opendrive_road_segment.cpp b/src/opendrive/opendrive_road_segment.cpp new file mode 100644 index 0000000000000000000000000000000000000000..61e28b2c5af3e64ea95a1f386b288573b7fc9ba5 --- /dev/null +++ b/src/opendrive/opendrive_road_segment.cpp @@ -0,0 +1,822 @@ +#include "opendrive/opendrive_road_segment.h" +#include "opendrive/opendrive_line.h" +#include "opendrive/opendrive_spiral.h" +#include "opendrive/opendrive_arc.h" +#include "opendrive/opendrive_param_poly3.h" +#include "road_segment.h" +#include "common.h" +#include "exceptions.h" +#include <math.h> + +COpendriveRoadSegment::COpendriveRoadSegment() +{ + this->name=""; + this->id=-1; + this->center_mark=OD_MARK_NONE; + this->left_lanes.clear(); + this->right_lanes.clear(); + this->signals.clear(); + this->objects.clear(); + this->geometries.clear(); + this->predecessor_exists=false; + this->successor_exists=false; + this->junction_id=-1; +} + +void COpendriveRoadSegment::free(void) +{ + for(unsigned int i=0;i<this->geometries.size();i++) + { + delete this->geometries[i]; + this->geometries[i]=NULL; + } + this->geometries.clear(); + for(unsigned int i=0;i<this->right_lanes.size();i++) + { + delete this->right_lanes[i]; + this->right_lanes[i]=NULL; + } + this->right_lanes.clear(); + for(unsigned int i=0;i<this->left_lanes.size();i++) + { + delete this->left_lanes[i]; + this->left_lanes[i]=NULL; + } + this->left_lanes.clear(); + for(unsigned int i=0;i<this->signals.size();i++) + { + delete this->signals[i]; + this->signals[i]=NULL; + } + this->signals.clear(); + for(unsigned int i=0;i<this->objects.size();i++) + { + delete this->objects[i]; + this->objects[i]=NULL; + } +} + +void COpendriveRoadSegment::set_name(const std::string &name) +{ + this->name=name; +} + +void COpendriveRoadSegment::set_id(unsigned int id) +{ + this->id=id; +} + +void COpendriveRoadSegment::set_center_mark(opendrive_mark_t mark) +{ + this->center_mark=mark; +} + +bool COpendriveRoadSegment::add_geometries(OpenDRIVE::road_type &road_info) +{ + planView::geometry_iterator geom_info; + COpendriveGeometry *geometry; + + for(unsigned int i=0;i<this->geometries.size();i++) + delete this->geometries[i]; + this->geometries.clear(); + for(geom_info=road_info.planView().geometry().begin(); geom_info!=road_info.planView().geometry().end(); ++geom_info) + { + // import geometry + if(geom_info->line().present()) + geometry=new COpendriveLine(); + else if(geom_info->spiral().present()) + geometry=new COpendriveSpiral(); + else if(geom_info->arc().present()) + geometry=new COpendriveArc(); + else if(geom_info->paramPoly3().present()) + geometry=new COpendriveParamPoly3(); + else + throw CException(_HERE_,"Unknown geometry"); + geometry->load(*geom_info); + this->geometries.push_back(geometry); + } + if(this->geometries.size()==0) + return false; + else + return true; +} + +void COpendriveRoadSegment::add_geometry(COpendriveGeometry *geometry,double resolution) +{ + TOpendriveWorldPose start_pose,end_pose; + + if(geometry==NULL) + throw CException(_HERE_,"Invalid geometry reference"); + if(this->geometries.size()==0) + this->geometries.push_back(geometry); + else + { + start_pose=geometry->get_start_pose(); + end_pose=this->geometries[this->geometries.size()-1]->get_end_pose(); + if(sqrt(pow(start_pose.x-end_pose.x,2.0)+pow(start_pose.y-end_pose.y,2.0))<=resolution) + this->geometries.push_back(geometry); + else + throw CException(_HERE_,"geometries do not coincide"); + } +} + +void COpendriveRoadSegment::set_junction_id(unsigned int id) +{ + this->junction_id=id; +} + +void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section) +{ + right::lane_iterator r_lane_it; + left::lane_iterator l_lane_it; + + // right lanes + if(lane_section.right().present()) + { + for(r_lane_it=lane_section.right()->lane().begin();r_lane_it!=lane_section.right()->lane().end();r_lane_it++) + this->add_right_lane(*r_lane_it); + } + // left lanes + if(lane_section.left().present()) + { + for(l_lane_it=lane_section.left()->lane().begin();l_lane_it!=lane_section.left()->lane().end();l_lane_it++) + this->add_left_lane(*l_lane_it); + } +} + +void COpendriveRoadSegment::add_right_lane(const ::lane &lane_info) +{ + COpendriveLane *new_lane; + + try{ + new_lane=new COpendriveLane(); + new_lane->load(lane_info); + for(unsigned int i=0;i<this->right_lanes.size();i++) + if(this->right_lanes[i]->get_id()==new_lane->get_id()) + throw CException(_HERE_,"A lane with the same ID has already been added"); + this->right_lanes.push_back(new_lane); + }catch(CException &e){ + delete new_lane; + throw e; + } +} + +void COpendriveRoadSegment::add_right_lane(COpendriveLane *lane) +{ + if(lane==NULL) + throw CException(_HERE_,"Invalid right lane reference"); + for(unsigned int i=0;i<this->right_lanes.size();i++) + if(this->right_lanes[i]->get_id()==lane->get_id()) + throw CException(_HERE_,"A lane with the same ID has already been added"); + this->right_lanes.push_back(lane); +} + +void COpendriveRoadSegment::add_left_lane(const ::lane &lane_info) +{ + COpendriveLane *new_lane; + + try{ + new_lane=new COpendriveLane(); + new_lane->load(lane_info); + for(unsigned int i=0;i<this->left_lanes.size();i++) + if(this->left_lanes[i]->get_id()==new_lane->get_id()) + throw CException(_HERE_,"A lane with the same ID has already been added"); + this->left_lanes.push_back(new_lane); + }catch(CException &e){ + delete new_lane; + throw e; + } +} + +void COpendriveRoadSegment::add_left_lane(COpendriveLane *lane) +{ + if(lane==NULL) + throw CException(_HERE_,"Invalid left lane reference"); + for(unsigned int i=0;i<this->left_lanes.size();i++) + if(this->left_lanes[i]->get_id()==lane->get_id()) + throw CException(_HERE_,"A lane with the same ID has already been added"); + this->left_lanes.push_back(lane); +} + +bool COpendriveRoadSegment::is_junction(void) const +{ + if(this->junction_id==(unsigned int)-1) + return false; + else + return true; +} + +bool COpendriveRoadSegment::has_predecessor(void) +{ + return this->predecessor_exists; +} + +bool COpendriveRoadSegment::is_predecessor_road(void) +{ + if(this->predecessor_exists) + return this->predecessor.road; + else + throw CException(_HERE_,"Road segment has no predecessor"); +} + +bool COpendriveRoadSegment::is_predecessor_junction(void) +{ + if(this->predecessor_exists) + return !this->predecessor.road; + else + throw CException(_HERE_,"Road segment has no predecessor"); +} + +unsigned int COpendriveRoadSegment::get_predecessor_id(void) +{ + if(this->predecessor_exists) + return this->predecessor.id; + else + throw CException(_HERE_,"Road segment has no predecessor"); +} + +bool COpendriveRoadSegment::is_predecessor_contact_end(void) +{ + if(this->predecessor_exists) + return this->predecessor.end_contact; + else + throw CException(_HERE_,"Road segment has no predecessor"); +} + +bool COpendriveRoadSegment::is_predecessor_contact_start(void) +{ + if(this->predecessor_exists) + return !this->predecessor.end_contact; + else + throw CException(_HERE_,"Road segment has no predecessor"); +} + +bool COpendriveRoadSegment::has_successor(void) +{ + return this->successor_exists; +} + +bool COpendriveRoadSegment::is_successor_road(void) +{ + if(this->successor_exists) + return this->successor.road; + else + throw CException(_HERE_,"Road segment has no successor"); +} + +bool COpendriveRoadSegment::is_successor_junction(void) +{ + if(this->successor_exists) + return !this->successor.road; + else + throw CException(_HERE_,"Road segment has no successor"); +} + +unsigned int COpendriveRoadSegment::get_successor_id(void) +{ + if(this->successor_exists) + return this->successor.id; + else + throw CException(_HERE_,"Road segment has no successor"); +} + +bool COpendriveRoadSegment::is_successor_contact_end(void) +{ + if(this->successor_exists) + return this->successor.end_contact; + else + throw CException(_HERE_,"Road segment has no successor"); +} + +bool COpendriveRoadSegment::is_successor_contact_start(void) +{ + if(this->successor_exists) + return !this->successor.end_contact; + else + throw CException(_HERE_,"Road segment has no successor"); +} + +void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info) +{ + signals::signal_iterator signal_it; + objects::object_iterator object_it; + lanes::laneSection_iterator lane_section; + COpendriveSignal *new_signal; + COpendriveObject *new_object; + std::stringstream error; + + this->free(); + this->set_name(road_info.name().get()); + this->set_id(std::stol(road_info.id().get())); + + // read link information + if(road_info.lane_link().present()) + { + if(road_info.lane_link().get().predecessor().present())// predecessor present + { + this->predecessor_exists=true; + if(road_info.lane_link().get().predecessor().get().elementType().get()=="road")// previous segment is a road + this->predecessor.road=true; + else + this->predecessor.road=false; + this->predecessor.id=std::stol(road_info.lane_link().get().predecessor().get().elementId().get()); + if(road_info.lane_link().get().predecessor().get().contactPoint().get()=="end") + this->predecessor.end_contact=true; + else + this->predecessor.end_contact=false; + } + else + this->predecessor_exists=false; + if(road_info.lane_link().get().successor().present())// successor present + { + this->successor_exists=true; + if(road_info.lane_link().get().successor().get().elementType().get()=="road")// previous segment is a road + this->successor.road=true; + else + this->successor.road=false; + this->successor.id=std::stol(road_info.lane_link().get().successor().get().elementId().get()); + if(road_info.lane_link().get().successor().get().contactPoint().get()=="end") + this->successor.end_contact=true; + else + this->successor.end_contact=false; + } + else + this->successor_exists=false; + } + // only one lane section is supported + if(road_info.lanes().laneSection().size()<1) + { + error << "No lane sections defined for segment " << this->name; + throw CException(_HERE_,error.str()); + } + else if(road_info.lanes().laneSection().size()>1) + { + error << "Segment " << this->name << " has more than one lane section"; + throw CException(_HERE_,error.str()); + } + else + lane_section=road_info.lanes().laneSection().begin(); + + try{ + // add lanes + this->add_lanes(*lane_section); + // add geometries + if(!this->add_geometries(road_info)) + { + error << "Segment " << this->name << " has no valid geometry"; + throw CException(_HERE_,error.str()); + } + // add road signals + if(road_info.signals().present()) + { + for(signal_it=road_info.signals().get().signal().begin();signal_it!=road_info.signals().get().signal().end();++signal_it) + { + new_signal=new COpendriveSignal(); + new_signal->load(*signal_it); + this->signals.push_back(new_signal); + } + } + // add road objects + if(road_info.objects().present()) + { + for(object_it=road_info.objects().get().object().begin();object_it!=road_info.objects().get().object().end();++object_it) + { + new_object=new COpendriveObject(); + new_object->load(*object_it); + this->objects.push_back(new_object); + } + } + this->junction_id=std::stol(road_info.junction().get()); + }catch(CException &e){ + this->free(); + throw e; + } +} + +void COpendriveRoadSegment::convert(CRoad **left_road,CRoad **right_road,double resolution) +{ + double avg_width,heading; + TOpendriveWorldPose start_point,end_point; + opendrive_mark_t mark; + CRoadSegment *segment; + bool first; + + if(this->right_lanes.size()>0) + { + (*right_road)=new CRoad(); + (*right_road)->set_num_lanes(this->right_lanes.size()); + avg_width=0.0; + for(unsigned int i=0;i<this->right_lanes.size();i++) + avg_width+=this->right_lanes[i]->get_width(); + avg_width/=this->right_lanes.size(); + (*right_road)->set_lane_width(avg_width); + first=true; + for(unsigned int i=0;i<this->geometries.size();i++) + { + if(this->geometries[i]->get_length()>resolution) + { + if(first) + { + start_point=this->geometries[i]->get_start_pose(); + (*right_road)->set_start_point(start_point.x,start_point.y,start_point.heading,0.0); + first=false; + } + end_point=this->geometries[i]->get_end_pose(); + (*right_road)->add_segment(end_point.x,end_point.y,end_point.heading,0.0); + segment=(*right_road)->get_last_segment(); + for(unsigned int j=0;j<this->right_lanes.size();j++) + { + mark=this->right_lanes[j]->get_right_road_mark(); + if(mark==OD_MARK_SOLID || mark==OD_MARK_SOLID_SOLID) + { + for(unsigned int k=abs(this->right_lanes[j]->get_id());k<this->right_lanes.size();k++) + { + segment->unlink_lanes(j,k); + segment->unlink_lanes(k,j); + } + } + else if(mark==OD_MARK_SOLID_BROKEN) + { + for(unsigned int k=abs(this->right_lanes[j]->get_id());k<this->right_lanes.size();k++) + segment->unlink_lanes(j,k); + } + else if(mark==OD_MARK_BROKEN_SOLID) + { + for(unsigned int k=abs(this->right_lanes[j]->get_id());k<this->right_lanes.size();k++) + segment->unlink_lanes(k,j); + } + } + } + } + } + else + (*right_road)=NULL; + if(this->left_lanes.size()>0) + { + (*left_road)=new CRoad(); + (*left_road)->set_num_lanes(this->left_lanes.size()); + avg_width=0.0; + for(unsigned int i=0;i<this->left_lanes.size();i++) + avg_width+=this->left_lanes[i]->get_width(); + avg_width/=this->left_lanes.size(); + (*left_road)->set_lane_width(avg_width); + first=true; + for(unsigned int i=0;i<this->geometries.size();i++) + { + if(this->geometries[this->geometries.size()-i-1]->get_length()>resolution) + { + if(first) + { + start_point=this->geometries[this->geometries.size()-i-1]->get_end_pose(); + heading=normalize_angle(start_point.heading+3.14159); + (*left_road)->set_start_point(start_point.x,start_point.y,heading,0.0); + first=false; + } + end_point=this->geometries[this->geometries.size()-i-1]->get_start_pose(); + heading=normalize_angle(end_point.heading+3.14159); + (*left_road)->add_segment(end_point.x,end_point.y,heading,0.0); + segment=(*left_road)->get_last_segment(); + for(unsigned int j=0;j<this->left_lanes.size();j++) + { + mark=this->left_lanes[j]->get_right_road_mark(); + if(mark==OD_MARK_SOLID || mark==OD_MARK_SOLID_SOLID) + { + for(unsigned int k=this->left_lanes[j]->get_id();k<this->left_lanes.size();k++) + { + segment->unlink_lanes(j,k); + segment->unlink_lanes(k,j); + } + } + else if(mark==OD_MARK_SOLID_BROKEN) + { + for(unsigned int k=this->left_lanes[j]->get_id();k<this->left_lanes.size();k++) + segment->unlink_lanes(j,k); + } + else if(mark==OD_MARK_BROKEN_SOLID) + { + for(unsigned int k=this->left_lanes[j]->get_id();k<this->left_lanes.size();k++) + segment->unlink_lanes(k,j); + } + } + } + } + } + else + (*left_road)=NULL; + if((*right_road)!=NULL && (*left_road)!=NULL) + { + (*left_road)->set_opposite_direction_road((*right_road)); + (*right_road)->set_opposite_direction_road((*left_road)); + } +} + +void COpendriveRoadSegment::convert(CRoad *left_road,CRoad *right_road,unsigned int ¤t_id,std::vector<COpendriveRoadSegment *> &segments) +{ + COpendriveLane *new_lane; + COpendriveRoadSegment *new_segment; + CRoadSegment *right_segment,*left_segment; + std::stringstream txt; + TPoint start_point,end_point; + TOpendriveWorldPose start_pose,end_pose; + COpendriveParamPoly3 *new_geometry; + + if(right_road==NULL) + throw CException(_HERE_,"Invalid right road reference"); + segments.clear(); + for(unsigned int i=0;i<right_road->get_num_segments();i++) + { + right_segment=right_road->get_segment_by_index(i); + new_segment=new COpendriveRoadSegment(); + txt.str(""); + txt << "road" << current_id; + new_segment->set_name(txt.str()); + new_segment->set_id(current_id); + new_segment->set_center_mark(OD_MARK_SOLID); + if(right_segment->has_parent_junction()) + new_segment->set_junction_id(right_segment->get_parent_junction().get_id()); + else + new_segment->set_junction_id(-1); + // add geometry + right_segment->get_start_point(start_point); + start_pose.x=start_point.x; + start_pose.y=start_point.y; + start_pose.heading=start_point.heading; + right_segment->get_end_point(end_point); + end_pose.x=end_point.x; + end_pose.y=end_point.y; + end_pose.heading=end_point.heading; + new_geometry=new COpendriveParamPoly3(start_pose,end_pose); + new_segment->add_geometry(new_geometry,right_road->get_resolution()); + // add right lanes + for(unsigned int j=0;j<right_road->get_num_lanes();j++) + { + new_lane=new COpendriveLane(); + new_lane->set_speed(right_road->get_lane_speed()); + new_lane->set_width(right_road->get_lane_width()); + new_lane->set_id(-(j+1)); + if(j==(right_road->get_num_lanes()-1)) + new_lane->set_right_mark(OD_MARK_SOLID); + else + { + if(right_segment->are_lanes_linked(j,j+1)) + new_lane->set_right_mark(OD_MARK_BROKEN); + else + new_lane->set_right_mark(OD_MARK_SOLID); + } + new_segment->add_right_lane(new_lane); + } + // add left lanes + if(left_road!=NULL) + { + left_segment=left_road->get_segment_by_index(left_road->get_num_segments()-1-i); + for(unsigned int j=0;j<left_road->get_num_lanes();j++) + { + new_lane=new COpendriveLane(); + new_lane->set_speed(left_road->get_lane_speed()); + new_lane->set_width(left_road->get_lane_width()); + new_lane->set_id(j+1); + if(j==(left_road->get_num_lanes()-1)) + new_lane->set_right_mark(OD_MARK_SOLID); + else + { + if(left_segment->are_lanes_linked(j,j+1)) + new_lane->set_right_mark(OD_MARK_BROKEN); + else + new_lane->set_right_mark(OD_MARK_SOLID); + } + new_segment->add_left_lane(new_lane); + } + } + // add connectivity + if(right_segment==right_road->get_first_segment()) + { + if(right_road->exist_prev_junction()) + { + new_segment->predecessor_exists=true; + new_segment->predecessor.road=false; + new_segment->predecessor.id=right_road->get_prev_junction().get_id(); + new_segment->predecessor.end_contact=false; + } + else + new_segment->predecessor_exists=false; + } + else + { + new_segment->predecessor_exists=true; + new_segment->predecessor.road=true; + new_segment->predecessor.id=current_id-1; + new_segment->predecessor.end_contact=false; + } + if(right_segment==right_road->get_last_segment()) + { + if(right_road->exist_next_junction()) + { + new_segment->successor_exists=true; + new_segment->successor.road=false; + new_segment->successor.id=right_road->get_next_junction().get_id(); + new_segment->successor.end_contact=true; + } + else + new_segment->successor_exists=false; + } + else + { + new_segment->successor_exists=true; + new_segment->successor.road=true; + new_segment->successor.id=current_id+1; + new_segment->successor.end_contact=true; + } + segments.push_back(new_segment); + current_id++; + } +} + +void COpendriveRoadSegment::save(OpenDRIVE::road_type **road_info) +{ + ::lanes lanes; + ::laneSection *section; + ::center center; + ::centerLane center_lane; + ::roadMark1 lane_mark; + ::left left_lanes; + ::right right_lanes; + ::type2 road_type; + ::planView plan_view; + ::link2 link; + ::predecessor1 predecessor; + ::successor1 successor; + + // center lane + center_lane.id(0); + center_lane.type("driving"); + center_lane.level("false"); + switch(this->center_mark) + { + case OD_MARK_NONE: + lane_mark.type1("none"); + break; + case OD_MARK_SOLID: + lane_mark.type1("solid"); + break; + case OD_MARK_BROKEN: + lane_mark.type1("broken"); + break; + case OD_MARK_SOLID_SOLID: + lane_mark.type1("solid solid"); + break; + case OD_MARK_SOLID_BROKEN: + lane_mark.type1("solid broken"); + break; + case OD_MARK_BROKEN_SOLID: + lane_mark.type1("broken solid"); + break; + case OD_MARK_BROKEN_BROKEN: + lane_mark.type1("broken broken"); + break; + default: + lane_mark.type1("none"); + break; + } + lane_mark.sOffset(0.0); + lane_mark.weight("standard"); + lane_mark.color("standard"); + lane_mark.width(0.2); + lane_mark.laneChange("both"); + center_lane.roadMark().push_back(lane_mark); + center.lane(center_lane); + section=new laneSection(center); + // lane section + section->s(0.0); + for(unsigned int i=0;i<this->left_lanes.size();i++) + { + ::lane new_lane; + this->left_lanes[i]->save(new_lane); + left_lanes.lane().push_back(new_lane); + } + section->left(left_lanes); + for(unsigned int i=0;i<this->right_lanes.size();i++) + { + ::lane new_lane; + this->right_lanes[i]->save(new_lane); + right_lanes.lane().push_back(new_lane); + } + section->right(right_lanes); + lanes.laneSection().push_back(*section); + delete section; + // geometry + for(unsigned int i=0;i<this->geometries.size();i++) + { + ::geometry new_geometry; + this->geometries[i]->save(new_geometry); + plan_view.geometry().push_back(new_geometry); + } + (*road_info)=new OpenDRIVE::road_type(plan_view,lanes); + (*road_info)->name(this->name); + (*road_info)->id(std::to_string(this->id)); + (*road_info)->length(this->get_length()); + // road type + road_type.s(0.0); + road_type.type("town"); + (*road_info)->type().push_back(road_type); + // link information + (*road_info)->junction(std::to_string((int)this->junction_id)); + /* get connectivity info from junction object */ + if(this->predecessor.road) + predecessor.elementType("road"); + else + predecessor.elementType("junction"); + predecessor.elementId(std::to_string(this->predecessor.id)); + if(this->predecessor.end_contact) + predecessor.contactPoint("end"); + else + predecessor.contactPoint("start"); + link.predecessor(predecessor); + if(this->successor.road) + successor.elementType("road"); + else + successor.elementType("junction"); + successor.elementId(std::to_string(this->successor.id)); + if(this->successor.end_contact) + successor.contactPoint("end"); + else + successor.contactPoint("start"); + link.successor(successor); + (*road_info)->lane_link(link); +} + +std::string COpendriveRoadSegment::get_name(void) const +{ + return this->name; +} + +unsigned int COpendriveRoadSegment::get_id(void) const +{ + return this->id; +} + +unsigned int COpendriveRoadSegment::get_num_right_lanes(void) const +{ + return this->right_lanes.size(); +} + +unsigned int COpendriveRoadSegment::get_num_left_lanes(void) const +{ + return this->left_lanes.size(); +} + +unsigned int COpendriveRoadSegment::get_num_signals(void) const +{ + return this->signals.size(); +} + +const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index) const +{ + std::stringstream error; + + if(index>=0 && index<this->signals.size()) + return *this->signals[index]; + else + { + error << "Invalid signal index (" << index << ") for segment " << this->name; + throw CException(_HERE_,error.str()); + } +} + +unsigned int COpendriveRoadSegment::get_num_objects(void) const +{ + return this->objects.size(); +} + +const COpendriveObject &COpendriveRoadSegment::get_object(unsigned int index) const +{ + std::stringstream error; + + if(index>=0 && index<this->objects.size()) + return *this->objects[index]; + else + { + error << "Invalid object index (" << index << ") for segment " << this->name; + throw CException(_HERE_,error.str()); + } +} + +double COpendriveRoadSegment::get_length(void) +{ + double length=0.0; + + for(unsigned int i=0;i<this->geometries.size();i++) + length+=this->geometries[i]->get_length(); + + return length; +} + +COpendriveRoadSegment::~COpendriveRoadSegment() +{ + this->free(); + this->objects.clear(); + this->name=""; + this->id=-1; + this->center_mark=OD_MARK_NONE; + this->junction_id=-1; +} + diff --git a/src/opendrive_signal.cpp b/src/opendrive/opendrive_signal.cpp similarity index 70% rename from src/opendrive_signal.cpp rename to src/opendrive/opendrive_signal.cpp index bea1acdf228e146f63b376d20800fd2430acd059..9ae4ff2a017b5f38d525514b319544a478bde3bf 100644 --- a/src/opendrive_signal.cpp +++ b/src/opendrive/opendrive_signal.cpp @@ -1,10 +1,10 @@ -#include "opendrive_signal.h" +#include "opendrive/opendrive_signal.h" +#include "common.h" #include "exceptions.h" #include <cmath> COpendriveSignal::COpendriveSignal() { - this->segment=NULL; this->id=-1; this->pose.s=0.0; this->pose.t=0.0; @@ -13,12 +13,10 @@ COpendriveSignal::COpendriveSignal() this->sub_type=""; this->value=0; this->text=""; - this->scale_factor=DEFAULT_SCALE_FACTOR; } COpendriveSignal::COpendriveSignal(const COpendriveSignal &object) { - this->segment=object.segment; this->id=object.id; this->pose.s=object.pose.s; this->pose.t=object.pose.t; @@ -27,10 +25,9 @@ COpendriveSignal::COpendriveSignal(const COpendriveSignal &object) this->sub_type=object.sub_type; this->value=object.value; this->text=object.text; - this->scale_factor=object.scale_factor; } -void COpendriveSignal::load(signals::signal_type &signal_info,COpendriveRoadSegment *segment) +void COpendriveSignal::load(signals::signal_type &signal_info) { std::stringstream ss(signal_info.id().get()); ss >> this->id; @@ -45,18 +42,6 @@ void COpendriveSignal::load(signals::signal_type &signal_info,COpendriveRoadSegm if (signal_info.orientation().present() && signal_info.orientation().get() == orientation::cxx_1) reverse = true; this->pose.heading = normalize_angle(this->pose.heading + (reverse ? M_PI : 0.0)); - this->segment=segment; -} - -void COpendriveSignal::update_references(segment_up_ref_t &segment_refs) -{ - if(segment_refs.find(this->segment)!=segment_refs.end()) - this->segment=segment_refs[this->segment]; -} - -void COpendriveSignal::set_scale_factor(double scale) -{ - this->scale_factor=scale; } int COpendriveSignal::get_id(void) const @@ -64,35 +49,17 @@ int COpendriveSignal::get_id(void) const return this->id; } -double COpendriveSignal::get_scale_factor(void) -{ - return this->scale_factor; -} - TOpendriveTrackPose COpendriveSignal::get_track_pose(void) const { TOpendriveTrackPose track; - track.s=this->pose.s/this->scale_factor; - track.t=this->pose.t/this->scale_factor; + track.s=this->pose.s; + track.t=this->pose.t; track.heading=this->pose.heading; return track; } -TOpendriveWorldPose COpendriveSignal::get_world_pose(void) const -{ - TOpendriveWorldPose world; - - if(this->segment!=NULL) - { - world=segment->transform_to_world_pose(this->get_track_pose()); - return world; - } - else - throw CException(_HERE_,"Invalid parent segment reference"); -} - void COpendriveSignal::get_type(std::string &type, std::string &sub_type) const { type=this->type; @@ -125,7 +92,6 @@ std::ostream& operator<<(std::ostream& out, COpendriveSignal &signal) COpendriveSignal::~COpendriveSignal() { - this->segment=NULL; this->id=-1; this->pose.s=0.0; this->pose.t=0.0; @@ -134,6 +100,5 @@ COpendriveSignal::~COpendriveSignal() this->sub_type=""; this->value=0; this->text=""; - this->scale_factor=DEFAULT_SCALE_FACTOR; } diff --git a/src/opendrive_spiral.cpp b/src/opendrive/opendrive_spiral.cpp similarity index 71% rename from src/opendrive_spiral.cpp rename to src/opendrive/opendrive_spiral.cpp index 089ad95045e80fd433633b0d09531642d50a8af4..ef648bb2b70f253a02e304cdc3d3e38c6a28661e 100644 --- a/src/opendrive_spiral.cpp +++ b/src/opendrive/opendrive_spiral.cpp @@ -1,4 +1,4 @@ -#include "opendrive_spiral.h" +#include "opendrive/opendrive_spiral.h" COpendriveSpiral::COpendriveSpiral() { @@ -12,6 +12,12 @@ COpendriveSpiral::COpendriveSpiral(const COpendriveSpiral &object) : COpendriveG this->end_curvature=object.end_curvature; } +COpendriveSpiral::COpendriveSpiral(TOpendriveWorldPose &start_pose,double length,double start_curvature,double end_curvature) : COpendriveGeometry(start_pose,length) +{ + this->start_curvature=start_curvature; + this->end_curvature=end_curvature; +} + bool COpendriveSpiral::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const { return true; @@ -32,6 +38,15 @@ void COpendriveSpiral::load_params(const planView::geometry_type &geometry_info) this->end_curvature = (geometry_info.spiral().get().curvEnd().present() ? geometry_info.spiral().get().curvEnd().get() : 0.0); } +void COpendriveSpiral::save_params(planView::geometry_type &geometry_info) +{ + ::spiral spiral; + + spiral.curvStart(this->start_curvature); + spiral.curvEnd(this->end_curvature); + geometry_info.spiral(spiral); +} + std::string COpendriveSpiral::get_name(void) { return std::string("spiral"); @@ -46,8 +61,13 @@ COpendriveGeometry *COpendriveSpiral::clone(void) void COpendriveSpiral::get_curvature(double &start,double &end) { - start=this->start_curvature*this->scale_factor; - end=this->end_curvature*this->scale_factor; + start=this->start_curvature; + end=this->end_curvature; +} + +void COpendriveSpiral::get_curvature_at(double length,double &curvature) +{ + curvature=0.0; } void COpendriveSpiral::operator=(const COpendriveSpiral &object) diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp deleted file mode 100644 index ddda090bae8d56c1654a5e53b98b0a50a3532c08..0000000000000000000000000000000000000000 --- a/src/opendrive_lane.cpp +++ /dev/null @@ -1,993 +0,0 @@ -#include "opendrive_lane.h" -#include "exceptions.h" -#include <math.h> - -COpendriveLane::COpendriveLane() -{ - this->nodes.clear(); - this->next.clear(); - this->prev.clear(); - this->left_lane=NULL; - this->left_mark=OD_MARK_NONE; - this->right_lane=NULL; - this->right_mark=OD_MARK_NONE; - 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; - this->id=0; - this->index=-1; -} - -COpendriveLane::COpendriveLane(const COpendriveLane &object) -{ - this->nodes=object.nodes; - this->next=object.next; - this->prev=object.prev; - this->left_lane=object.left_lane; - this->left_mark=object.left_mark; - this->right_lane=object.right_lane; - this->right_mark=object.right_mark; - this->segment=object.segment; - this->resolution=object.resolution; - this->scale_factor=object.scale_factor; - this->width=object.width; - this->speed=object.speed; - this->offset=object.offset; - this->id=object.id; - this->index=object.index; -} - -void COpendriveLane::link_neightbour_lane(COpendriveLane *lane) -{ - unsigned int min_num_nodes; - - if(lane!=NULL) - { - if(this->get_num_nodes()<lane->get_num_nodes()) - min_num_nodes=this->get_num_nodes(); - else - min_num_nodes=lane->get_num_nodes(); - for(unsigned int i=0;i<min_num_nodes-1;i++) - { - this->nodes[i]->add_link(lane->nodes[i+1],lane->right_mark,this->segment,NULL); - lane->nodes[i]->add_link(this->nodes[i+1],lane->right_mark,this->segment,NULL); - this->left_mark=lane->right_mark; - } - if(min_num_nodes>0) - { - this->left_lane=lane; - lane->right_lane=this; - } - } -} - -void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start, bool to_start,bool belongs_to_lane) -{ - COpendriveRoadNode *start,*end; - std::stringstream error; - - if(lane!=NULL) - { - if(this->get_num_nodes()>0 && lane->get_num_nodes()>0) - { - 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]; - } - } - if(belongs_to_lane) - start->add_link(end,mark,this->segment,this); - else - start->add_link(end,mark,this->segment,NULL); - // link lane - this->add_next_lane(lane); - lane->add_prev_lane(this); - } - else - { - error << "Lane " << this->id << " of segment " << this->segment->get_name() << " or lane " << lane->get_id() << " of segment " << lane->get_segment().get_name() << " has no nodes"; - throw CException(_HERE_,error.str()); - } - } -} - -void COpendriveLane::add_next_lane(COpendriveLane *lane) -{ - for(unsigned int i=0;i<this->next.size();i++) - if(this->next[i]==lane)// lane is already linked - return; - - // add the lane - this->next.push_back(lane); -} - -void COpendriveLane::add_prev_lane(COpendriveLane *lane) -{ - for(unsigned int i=0;i<this->prev.size();i++) - if(this->prev[i]==lane)// lane is already linked - return; - - // add the lane - this->prev.push_back(lane); -} - -void COpendriveLane::remove_lane(COpendriveLane *lane) -{ - std::vector<COpendriveLane *>::iterator lane_it; - - // remove the reference from neightbour lanes - if(this->right_lane==lane) - this->right_lane=NULL; - if(this->left_lane==lane) - this->left_lane=NULL; - // remove the reference from next lanes - for(lane_it=this->next.begin();lane_it!=this->next.end();) - { - if((*lane_it)==lane) - lane_it=this->next.erase(lane_it); - else - lane_it++; - } - // remove the reference from next lanes - for(lane_it=this->prev.begin();lane_it!=this->prev.end();) - { - if((*lane_it)==lane) - lane_it=this->prev.erase(lane_it); - else - lane_it++; - } -} - -void COpendriveLane::add_node(COpendriveRoadNode *node) -{ - if(this->has_node(node)) - return; - // 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,this->segment,this); - // add the new node - this->nodes.push_back(node); -} - -bool COpendriveLane::has_node(COpendriveRoadNode *node) -{ - for(unsigned int i=0;i<this->nodes.size();i++) - if(this->nodes[i]==node) - return true; - - return false; -} - -bool COpendriveLane::has_node_with_index(unsigned int index) -{ - for(unsigned int i=0;i<this->nodes.size();i++) - if(this->nodes[i]->get_index()==index) - return true; - - return false; -} - -void COpendriveLane::set_parent_segment(COpendriveRoadSegment *segment) -{ - this->segment=segment; -} - -void COpendriveLane::set_left_lane(COpendriveLane *lane,opendrive_mark_t mark) -{ - this->left_lane=lane; - this->left_mark=mark; -} - -void COpendriveLane::set_right_lane(COpendriveLane *lane,opendrive_mark_t mark) -{ - this->right_lane=lane; - this->right_mark=mark; -} - -void COpendriveLane::set_resolution(double res) -{ - this->resolution=res; - - 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_width(double width) -{ - this->width=width; -} - -void COpendriveLane::set_speed(double speed) -{ - this->speed=speed; -} - -void COpendriveLane::set_offset(double offset) -{ - this->offset=offset; -} - -void COpendriveLane::set_id(int id) -{ - this->id=id; -} - -void COpendriveLane::set_index(unsigned int index) -{ - this->index=index; -} - -bool COpendriveLane::updated(lane_up_ref_t &refs) -{ - lane_up_ref_t::iterator updated_it; - - for(updated_it=refs.begin();updated_it!=refs.end();updated_it++) - if(updated_it->second==this) - return true; - - return false; -} - -void COpendriveLane::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs) -{ - std::vector<COpendriveLane *>::iterator lane_it; - std::vector<COpendriveRoadNode *>::iterator node_it; - - if(this->updated(lane_refs)) - { - 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]; - (*node_it)->update_references(segment_refs,lane_refs,node_refs); - } - else if((*node_it)->updated(node_refs)) - (*node_it)->update_references(segment_refs,lane_refs,node_refs); - } - for(lane_it=this->next.begin();lane_it!=this->next.end();lane_it++) - if(lane_refs.find(*lane_it)!=lane_refs.end()) - (*lane_it)=lane_refs[*lane_it]; - for(lane_it=this->prev.begin();lane_it!=this->prev.end();lane_it++) - if(lane_refs.find(*lane_it)!=lane_refs.end()) - (*lane_it)=lane_refs[*lane_it]; - if(lane_refs.find(this->left_lane)!=lane_refs.end()) - this->left_lane=lane_refs[this->left_lane]; - if(lane_refs.find(this->right_lane)!=lane_refs.end()) - this->right_lane=lane_refs[this->right_lane]; - if(segment_refs.find(this->segment)!=segment_refs.end()) - this->segment=segment_refs[this->segment]; - } -} - -void COpendriveLane::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs) -{ - std::vector<COpendriveLane *>::iterator lane_it; - std::vector<COpendriveRoadNode *>::iterator node_it; - - if(this->updated(lane_refs)) - { - for(node_it=this->nodes.begin();node_it!=this->nodes.end();) - { - if((*node_it)->updated(node_refs)) - { - (*node_it)->clean_references(segment_refs,lane_refs,node_refs); - node_it++; - } - else - node_it=this->nodes.erase(node_it); - } - for(lane_it=this->next.begin();lane_it!=this->next.end();) - { - if(!(*lane_it)->updated(lane_refs)) - lane_it=this->next.erase(lane_it); - else - lane_it++; - } - for(lane_it=this->prev.begin();lane_it!=this->prev.end();) - { - if(!(*lane_it)->updated(lane_refs)) - lane_it=this->prev.erase(lane_it); - else - lane_it++; - } - if(lane_refs.find(this->left_lane)!=lane_refs.end()) - this->left_lane=lane_refs[this->left_lane]; - else if(!this->left_lane->updated(lane_refs)) - this->left_lane=NULL; - if(lane_refs.find(this->right_lane)!=lane_refs.end()) - this->right_lane=lane_refs[this->right_lane]; - else if(!this->right_lane->updated(lane_refs)) - this->right_lane=NULL; - } -} - -void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start) -{ - std::vector<COpendriveRoadNode *>::iterator it; - segment_up_ref_t new_segment_ref; - unsigned int start_node_index,i; - TOpendriveWorldPose start_pose; - COpendriveRoadNode *new_node; - std::stringstream error; - double distance; - - if(start==NULL) - return; - start_node_index=this->get_closest_node_index(*start,distance,3.14159); - if(start_node_index==(unsigned int)-1) - { - error << "No node close to (x=" << start->x << ",y=" << start->y << ",heading=" << start->heading << ") in lane " << this->id << " of segment " << this->segment->get_name(); - throw CException(_HERE_,error.str()); - } - // get the actual start position on the lane - this->get_closest_pose(*start,start_pose,3.14159); - // eliminate all the node before the start one - for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++) - { - if(i<start_node_index) - { - if((*it)->updated(new_node_ref)) - { - new_node_ref.erase((*it)->get_original_node(new_node_ref)); - delete *it; - } - it=this->nodes.erase(it); - } - else - { - if(!(*it)->updated(new_node_ref)) - { - new_node=(*it)->clone(new_link_ref); - new_node_ref[*it]=new_node; - } - it++; - } - } - this->prev.clear();// it is no longer connected to any previous lane - this->update_references(new_segment_ref,new_lane_ref,new_node_ref); - this->nodes[0]->update_start_pose(this,start_pose,distance); -} - -void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *end) -{ - std::vector<COpendriveRoadNode *>::iterator it; - segment_up_ref_t new_segment_ref; - unsigned int end_node_index,i; - TOpendriveWorldPose end_pose; - COpendriveRoadNode *new_node; - std::stringstream error; - double distance; - - if(end==NULL) - return; - end_node_index=this->get_closest_node_index(*end,distance,3.14159); - if(end_node_index==(unsigned int)-1) - { - error << "No node close to (x=" << end->x << ",y=" << end->y << ",heading=" << end->heading << ") in lane " << this->id << " of segment " << this->segment->get_name(); - throw CException(_HERE_,error.str()); - } - this->get_closest_pose(*end,end_pose,3.14159); - // eliminate all the node before the start one - for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++) - { - if(i>end_node_index) - { - if((*it)->updated(new_node_ref)) - { - new_node_ref.erase((*it)->get_original_node(new_node_ref)); - delete *it; - } - it=this->nodes.erase(it); - } - else - { - if(!(*it)->updated(new_node_ref)) - { - new_node=(*it)->clone(new_link_ref); - new_node_ref[*it]=new_node; - } - it++; - } - } - this->next.clear();// it is no longer connected to any next lane - this->update_references(new_segment_ref,new_lane_ref,new_node_ref); - this->nodes[this->nodes.size()-1]->update_end_pose(this,end_pose,distance); -} - -COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start,TOpendriveWorldPose *end) -{ - COpendriveLane *new_lane; - - if(start==NULL && end==NULL) - return this->clone(new_node_ref,new_lane_ref,new_link_ref); - new_lane=new COpendriveLane(*this); - new_lane_ref[this]=new_lane; - if(this->id<0) - { - new_lane->update_start_node(new_node_ref,new_lane_ref,new_link_ref,start); - new_lane->update_end_node(new_node_ref,new_lane_ref,new_link_ref,end); - } - else - { - new_lane->update_start_node(new_node_ref,new_lane_ref,new_link_ref,end); - new_lane->update_end_node(new_node_ref,new_lane_ref,new_link_ref,start); - } - - return new_lane; -} - -COpendriveLane *COpendriveLane::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref) -{ - COpendriveLane *new_lane; - std::vector<COpendriveRoadNode *>::iterator it; - COpendriveRoadNode *new_node; - segment_up_ref_t new_segment_ref; - - new_lane=new COpendriveLane(*this); - new_lane_ref[this]=new_lane; - for(it=new_lane->nodes.begin();it!=new_lane->nodes.end();it++) - { - new_node=(*it)->clone(new_link_ref); - new_node_ref[*it]=new_node; - } - this->update_references(new_segment_ref,new_lane_ref,new_node_ref); - - return new_lane; -} - -void COpendriveLane::load(const ::lane &lane_info,COpendriveRoadSegment *segment) -{ - std::stringstream error; - opendrive_mark_t mark; - - this->nodes.clear(); - this->prev.clear(); - this->next.clear(); - this->left_lane=NULL; - this->left_mark=OD_MARK_NONE; - this->right_lane=NULL; - this->right_mark=OD_MARK_NONE; - this->set_id(lane_info.id().get()); - // import lane width - if(lane_info.width().size()<1) - { - error << "Lane " << this->id << " of segment " << segment->get_name() << " has no width record"; - throw CException(_HERE_,error.str()); - } - else if(lane_info.width().size()>1) - { - error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one width record"; - throw CException(_HERE_,error.str()); - } - this->set_width(lane_info.width().begin()->a().get()); - // import lane speed - if(lane_info.speed().size()<1) - this->set_speed(60.0); - else if(lane_info.speed().size()>1) - { - error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one speed record"; - throw CException(_HERE_,error.str()); - } - else - this->set_speed(lane_info.speed().begin()->max().get()); - //lane mark - if(lane_info.roadMark().size()==0) - mark=OD_MARK_NONE; - else if(lane_info.roadMark().size()>1) - { - error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one road mark record"; - throw CException(_HERE_,error.str()); - } - else if(lane_info.roadMark().size()==1) - { - 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; - else if(lane_info.roadMark().begin()->type1().get()=="solid solid") - mark=OD_MARK_SOLID_SOLID; - else if(lane_info.roadMark().begin()->type1().get()=="solid broken") - mark=OD_MARK_SOLID_BROKEN; - else if(lane_info.roadMark().begin()->type1().get()=="broken solid") - mark=OD_MARK_BROKEN_SOLID; - else if(lane_info.roadMark().begin()->type1().get()=="broken broken") - mark=OD_MARK_BROKEN_BROKEN; - else - mark=OD_MARK_NONE; - } - else - mark=OD_MARK_NONE; - } - this->right_mark=mark; - - this->set_parent_segment(segment); -} - -unsigned int COpendriveLane::get_num_nodes(void) const -{ - return this->nodes.size(); -} - -const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index) const -{ - std::stringstream error; - - if(index>=0 && index<this->nodes.size()) - return *this->nodes[index]; - else - { - error << "Invalid node index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; - throw CException(_HERE_,error.str()); - } -} - -unsigned int COpendriveLane::get_num_next_lanes(void) const -{ - return this->next.size(); -} - -const COpendriveLane &COpendriveLane::get_next_lane(unsigned int index) const -{ - std::stringstream error; - - if(index>=0 && index<this->next.size()) - return *this->next[index]; - else - { - error << "Invalid next lane index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; - throw CException(_HERE_,error.str()); - } -} - -unsigned int COpendriveLane::get_num_prev_lanes(void) const -{ - return this->prev.size(); -} - -const COpendriveLane &COpendriveLane::get_prev_lane(unsigned int index) const -{ - std::stringstream error; - - if(index>=0 && index<this->prev.size()) - return *this->prev[index]; - else - { - error << "Invalid previous lane index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; - throw CException(_HERE_,error.str()); - } -} - -const COpendriveRoadSegment &COpendriveLane::get_segment(void) const -{ - return *this->segment; -} - -bool COpendriveLane::exist_left_lane(void) -{ - if(this->left_lane!=NULL) - return true; - else - return false; -} - -const COpendriveLane &COpendriveLane::get_left_lane(void) const -{ - std::stringstream error; - - if(this->left_lane!=NULL) - return *this->left_lane; - - error << "Left lane not defined for segment " << this->segment->get_name() << " (lane " << this->id << ")"; - - throw CException(_HERE_,error.str()); -} - -bool COpendriveLane::exist_right_lane(void) -{ - if(this->right_lane!=NULL) - return true; - else - return false; -} - -const COpendriveLane &COpendriveLane::get_right_lane(void) const -{ - std::stringstream error; - - if(this->right_lane!=NULL) - return *this->right_lane; - - error << "Right lane not defined for segment " << this->segment->get_name() << " (lane " << this->id << ")"; - - throw CException(_HERE_,error.str()); -} - -opendrive_mark_t COpendriveLane::get_left_road_mark(void) const -{ - return this->left_mark; -} - -opendrive_mark_t COpendriveLane::get_right_road_mark(void) const -{ - return this->right_mark; -} - -double COpendriveLane::get_width(void) const -{ - return this->width/this->scale_factor; -} - -double COpendriveLane::get_speed(void) const -{ - return this->speed; -} - -double COpendriveLane::get_center_offset(void) const -{ - if(this->id<0) - return (this->offset-this->width/2.0)/this->scale_factor; - else - return (this->offset+this->width/2.0)/this->scale_factor; -} - -unsigned int COpendriveLane::get_index(void) const -{ - return this->index; -} - -int COpendriveLane::get_id(void) const -{ - return this->id; -} - -TOpendriveWorldPose COpendriveLane::get_start_pose(void) const -{ - TOpendriveTrackPose track_pose; - TOpendriveWorldPose world_pose; - std::stringstream error; - - track_pose.t=0.0; - if(this->id<0)// right lane - { - track_pose.s=0.0; - track_pose.heading=0.0; - } - else - { - track_pose.s=this->segment->get_length(); - track_pose.heading=3.14159; - } - world_pose=this->transform_to_world_pose(track_pose); - - return world_pose; -} - -TOpendriveWorldPose COpendriveLane::get_end_pose(void) const -{ - TOpendriveTrackPose track_pose; - TOpendriveWorldPose world_pose; - std::stringstream error; - - track_pose.t=0.0; - if(this->id<0)// right_lane - { - track_pose.s=this->segment->get_length(); - track_pose.heading=0.0; - } - else - { - track_pose.s=0.0; - track_pose.heading=3.14159; - } - world_pose=this->transform_to_world_pose(track_pose); - - return world_pose; -} - -double COpendriveLane::get_length(void) const -{ - std::stringstream error; - 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_links();j++) - { - if(&this->nodes[i]->get_link(j).get_parent_lane()==this) - length+=this->nodes[i]->links[j]->get_length(); - } - } - - return length; -} - -TOpendriveWorldPose COpendriveLane::get_pose_at(double length) const -{ - TOpendriveWorldPose world_pose={0}; - - for(unsigned int i=0;i<this->nodes.size();i++) - { - for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++) - { - if(&this->nodes[i]->get_link(j).get_parent_lane()==this) - { - if((length-this->nodes[i]->links[j]->get_length())<0.0) - { - world_pose=this->nodes[i]->links[j]->get_pose_at(length); - return world_pose; - } - else - length-=this->nodes[i]->links[j]->get_length(); - } - } - } - - return world_pose; -} - -double COpendriveLane::get_curvature_at(double length) const -{ - double curvature; - - for(unsigned int i=0;i<this->nodes.size();i++) - { - for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++) - { - if(&this->nodes[i]->get_link(j).get_parent_lane()==this) - { - if((length-this->nodes[i]->links[j]->get_length())<0.0) - { - curvature=this->nodes[i]->links[j]->get_curvature_at(length); - return curvature; - } - else - length-=this->nodes[i]->links[j]->get_length(); - } - } - } - - return 0.0; -} - -TOpendriveLocalPose COpendriveLane::transform_to_local_pose(TOpendriveTrackPose &track) const -{ - TOpendriveTrackPose pose; - - pose=track; - pose.t+=this->get_center_offset(); - return this->segment->transform_to_local_pose(pose); -} - -TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose &track) const -{ - TOpendriveTrackPose pose; - - pose=track; - pose.t+=this->get_center_offset(); - return this->segment->transform_to_world_pose(pose); -} - -unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const -{ - double dist,min_dist=std::numeric_limits<double>::max(),length; - TOpendriveWorldPose found_pose; - unsigned int closest_index=-1; - - for(unsigned int i=0;i<this->nodes.size();i++) - { - length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol); - if(length<std::numeric_limits<double>::max()) - { - dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0)); - if(dist<min_dist) - { - min_dist=dist; - closest_index=i; - distance=length; - } - } - } - - return closest_index; -} - -const COpendriveRoadNode &COpendriveLane::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]; -} - -double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const -{ - double dist,min_dist=std::numeric_limits<double>::max(),distance,length; - TOpendriveWorldPose found_pose; - unsigned int closest_index=-1; - COpendriveLink *link; - - closest_pose.x=std::numeric_limits<double>::max(); - closest_pose.y=std::numeric_limits<double>::max(); - closest_pose.heading=std::numeric_limits<double>::max(); - for(unsigned int i=0;i<this->nodes.size();i++) - { - length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol); - if(length<std::numeric_limits<double>::max()) - { - dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0)); - if(dist<min_dist) - { - min_dist=dist; - closest_index=i; - closest_pose=found_pose; - distance=length; - } - } - } - - for(unsigned int i=0;i<this->nodes.size();i++) - { - if(i<closest_index) - { - link=this->nodes[i]->get_link_with(this->nodes[i+1]); - if(link!=NULL) - distance+=link->get_length(); - } - else - break; - } - - return distance; -} - -std::ostream& operator<<(std::ostream& out, COpendriveLane &lane) -{ - out << " Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl; - out << " width: " << lane.get_width() << std::endl; - out << " speed: " << lane.get_speed() << std::endl; - out << " Previous lanes: " << std::endl; - for(unsigned int i=0;i<lane.prev.size();i++) - out << " Lane " << lane.prev[i]->id << " in road segment " << lane.prev[i]->segment->get_name() << std::endl; - out << " Next lanes: " << std::endl; - for(unsigned int i=0;i<lane.next.size();i++) - out << " Lane " << lane.next[i]->id << " in road segment " << lane.next[i]->segment->get_name() << std::endl; - if(lane.segment!=NULL) - out << " Parent road segment: " << lane.segment->get_name() << std::endl; - else - out << " No parent segment" << std::endl; - if(lane.left_lane==NULL) - out << " No left lane ("; - else - out << " Left lane " << lane.left_lane->get_id() << " ("; - 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; - } - if(lane.right_lane==NULL) - out << " No right lane ("; - else - out << " Right lane " << lane.right_lane->get_id() << " ("; - 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; - } - out << " Number of nodes: " << lane.nodes.size() << std::endl; - for(unsigned int i=0;i<lane.nodes.size();i++) - out << *lane.nodes[i]; - - return out; -} - -COpendriveLane::~COpendriveLane() -{ - this->nodes.clear(); - this->prev.clear(); - this->next.clear(); - this->left_lane=NULL; - this->left_mark=OD_MARK_NONE; - this->right_lane=NULL; - this->right_mark=OD_MARK_NONE; - 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; - this->id=0; - this->index=-1; -} diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp deleted file mode 100644 index 4243d05e6817746bb4667beb526355bf1671c681..0000000000000000000000000000000000000000 --- a/src/opendrive_link.cpp +++ /dev/null @@ -1,359 +0,0 @@ -#include "opendrive_link.h" -#include "opendrive_road_node.h" -#include "exceptions.h" - -COpendriveLink::COpendriveLink() -{ - this->prev=NULL; - this->next=NULL; - this->segment=NULL; - this->lane=NULL; - this->spline=NULL; - this->mark=OD_MARK_NONE; - this->resolution=DEFAULT_RESOLUTION; - this->scale_factor=DEFAULT_SCALE_FACTOR; -} - -COpendriveLink::COpendriveLink(const COpendriveLink &object) -{ - this->prev=object.prev; - this->next=object.next; - this->segment=object.segment; - this->lane=object.lane; - this->spline=new CG2Spline(*object.spline); - this->mark=object.mark; - this->resolution=object.resolution; - this->scale_factor=object.scale_factor; -} - -void COpendriveLink::set_prev(COpendriveRoadNode *node) -{ - this->prev=node; -} - -void COpendriveLink::set_next(COpendriveRoadNode *node) -{ - this->next=node; -} - -void COpendriveLink::set_road_mark(opendrive_mark_t mark) -{ - this->mark=mark; -} - -void COpendriveLink::set_parent_segment(COpendriveRoadSegment *segment) -{ - this->segment=segment; -} - -void COpendriveLink::set_parent_lane(COpendriveLane *lane) -{ - this->lane=lane; -} - -void COpendriveLink::set_resolution(double res) -{ - this->resolution=res; - - if(this->spline!=NULL) - this->spline->generate(res); -} - -void COpendriveLink::set_scale_factor(double scale) -{ - TPoint spline_start,spline_end; - - if(this->spline!=NULL) - { - this->spline->get_start_point(spline_start); - spline_start.x*=this->scale_factor/scale; - spline_start.y*=this->scale_factor/scale; - spline_start.curvature*=scale/this->scale_factor; - this->spline->set_start_point(spline_start); - this->spline->get_end_point(spline_end); - spline_end.x*=this->scale_factor/scale; - spline_end.y*=this->scale_factor/scale; - spline_end.curvature*=scale/this->scale_factor; - this->spline->set_end_point(spline_end); - this->spline->generate(this->resolution); - } - - this->scale_factor=scale; -} - -void COpendriveLink::generate(double start_curvature,double end_curvature) -{ - TOpendriveWorldPose node_start,node_end; - TPoint start,end; - - node_start=this->prev->get_pose(); - start.x=node_start.x; - start.y=node_start.y; - start.heading=node_start.heading; - start.curvature=start_curvature; - node_end=this->next->get_pose(); - end.x=node_end.x; - end.y=node_end.y; - end.heading=node_end.heading; - end.curvature=end_curvature; - this->spline=new CG2Spline(start,end); - this->spline->generate(this->resolution); -} - -void COpendriveLink::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs) -{ - if(node_refs.find(this->prev)!=node_refs.end()) - this->prev=node_refs[this->prev]; - if(node_refs.find(this->next)!=node_refs.end()) - this->next=node_refs[this->next]; - if(segment_refs.find(this->segment)!=segment_refs.end()) - this->segment=segment_refs[this->segment]; - if(lane_refs.find(this->lane)!=lane_refs.end()) - this->lane=lane_refs[this->lane]; -} - -bool COpendriveLink::clean_references(node_up_ref_t &refs) -{ - if(refs.find(this->prev)!=refs.end()) - this->prev=refs[this->prev]; - else if(!this->prev->updated(refs)) - return true; - if(refs.find(this->next)!=refs.end()) - this->next=refs[this->next]; - else if(!this->next->updated(refs)) - return true; - - return false; -} - -void COpendriveLink::update_start_pose(TOpendriveWorldPose &start,double curvature) -{ - TPoint start_pose; - - if(this->spline!=NULL) - { - start_pose.x=start.x; - start_pose.y=start.y; - start_pose.heading=start.heading; - start_pose.curvature=curvature; - this->spline->set_start_point(start_pose); - this->spline->generate(this->resolution); - } -} - -void COpendriveLink::update_end_pose(TOpendriveWorldPose &end,double curvature) -{ - TPoint end_pose; - - if(this->spline!=NULL) - { - end_pose.x=end.x; - end_pose.y=end.y; - end_pose.heading=end.heading; - end_pose.curvature=curvature; - this->spline->set_end_point(end_pose); - this->spline->generate(this->resolution); - } -} - -const COpendriveRoadNode &COpendriveLink::get_prev(void) const -{ - if(this->prev!=NULL) - return *this->prev; - - throw CException(_HERE_,"Invalid previous road node reference. Link is not properly defined"); -} - -const COpendriveRoadNode &COpendriveLink::get_next(void) const -{ - if(this->next!=NULL) - return *this->next; - - throw CException(_HERE_,"Invalid next road node reference. Link is not properly defined"); -} - -opendrive_mark_t COpendriveLink::get_road_mark(void) const -{ - return this->mark; -} - -const COpendriveRoadSegment &COpendriveLink::get_parent_segment(void) const -{ - if(this->segment!=NULL) - return *this->segment; - - throw CException(_HERE_,"Invalid parent road segment reference. Link is not properky defined"); -} - -const COpendriveLane &COpendriveLink::get_parent_lane(void) const -{ - std::stringstream error; - - if(this->lane!=NULL) - return *this->lane; - else - { - error << "Link from node " << this->prev->get_index() << " to node " << this->next->get_index() << " has no parent lane"; - throw CException(_HERE_,error.str()); - } -} - -bool COpendriveLink::is_lane_link(void) const -{ - if(this->lane==NULL) - return false; - else - return true; -} - -double COpendriveLink::get_resolution(void) const -{ - return this->resolution; -} - -double COpendriveLink::get_scale_factor(void) const -{ - return this->scale_factor; -} - -double COpendriveLink::find_closest_world_pose(TOpendriveWorldPose &world,TOpendriveWorldPose &pose) const -{ - TPoint spline_pose; - double length; - - if(this->spline!=NULL) - { - length=this->spline->find_closest_point(world.x,world.y,spline_pose); - pose.x=spline_pose.x; - pose.y=spline_pose.y; - pose.heading=normalize_angle(spline_pose.heading); - } - else - length=std::numeric_limits<double>::max(); - - return length; -} - -void COpendriveLink::get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length, double end_length) const -{ - std::vector<double> curvature,heading; - CG2Spline *partial_spline=NULL; - - if(start_length!=0.0 || end_length!=-1.0)// get partial spline - { - partial_spline=new CG2Spline; - this->spline->get_part(partial_spline,start_length,end_length); - partial_spline->evaluate_all(x,y,curvature,heading); - delete partial_spline; - } - else - this->spline->evaluate_all(x,y,curvature,heading); -} - -void COpendriveLink::get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length, double end_length) const -{ - std::vector<double> curvature; - CG2Spline *partial_spline=NULL; - - if(start_length!=0.0 || end_length!=-1.0)// get partial spline - { - partial_spline=new CG2Spline; - this->spline->get_part(partial_spline,start_length,end_length); - partial_spline->evaluate_all(x,y,curvature,yaw); - delete partial_spline; - } - else - this->spline->evaluate_all(x,y,curvature,yaw); -} - -double COpendriveLink::get_length(void) const -{ - if(this->spline!=NULL) - return this->spline->get_length(); - else - return 0.0; -} - -TOpendriveWorldPose COpendriveLink::get_pose_at(double length) -{ - TPoint spline_pose; - TOpendriveWorldPose world_pose={0}; - - if(this->spline!=NULL) - { - spline_pose=this->spline->evaluate(length); - world_pose.x=spline_pose.x; - world_pose.y=spline_pose.y; - world_pose.heading=spline_pose.heading; - } - - return world_pose; -} - -double COpendriveLink::get_curvature_at(double length) -{ - TPoint spline_pose; - double curvature=0.0; - - if(this->spline!=NULL) - { - spline_pose=this->spline->evaluate(length); - curvature=spline_pose.curvature; - } - - return curvature; -} - -std::ostream& operator<<(std::ostream& out, COpendriveLink &link) -{ - out << " Previous node: " << link.get_prev().get_index() << std::endl; - out << " Next node: " << link.get_next().get_index() << std::endl; - if(link.lane!=NULL) - out << " Parent segment: " << link.get_parent_segment().get_name() << " (lane " << link.get_parent_lane().get_id() << ")" << std::endl; - else - out << " Parent segment: " << link.get_parent_segment().get_name() << " (no lane)" << std::endl; - out << " Road mark: "; - switch(link.get_road_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; - } - - return out; -} - -COpendriveLink::~COpendriveLink() -{ - this->prev=NULL; - this->next=NULL; - this->segment=NULL; - this->lane=NULL; - if(this->spline!=NULL) - { - delete this->spline; - this->spline=NULL; - } - this->mark=OD_MARK_NONE; - this->resolution=DEFAULT_RESOLUTION; - this->scale_factor=DEFAULT_SCALE_FACTOR; -} - diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp deleted file mode 100644 index bb740b7e84fbb7af6e122ac865ee184e15139723..0000000000000000000000000000000000000000 --- a/src/opendrive_road.cpp +++ /dev/null @@ -1,1150 +0,0 @@ -#include "opendrive_road.h" -#include "exceptions.h" -#include <sys/types.h> -#include <sys/stat.h> -#include <math.h> - -COpendriveRoad::COpendriveRoad() -{ - this->resolution=DEFAULT_RESOLUTION; - this->scale_factor=DEFAULT_SCALE_FACTOR; - this->min_road_length=DEFAULT_MIN_ROAD_LENGTH; - this->segments.clear(); - this->nodes.clear(); - this->lanes.clear(); -} - -COpendriveRoad::COpendriveRoad(const COpendriveRoad& object) -{ - COpendriveRoadSegment *segment; - COpendriveRoadNode *node; - COpendriveLane *lane; - segment_up_ref_t new_segment_ref; - node_up_ref_t new_node_ref; - lane_up_ref_t new_lane_ref; - - this->free(); - this->resolution=object.resolution; - this->scale_factor=object.scale_factor; - this->min_road_length=object.min_road_length; - 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; - } - for(unsigned int i=0;i<object.nodes.size();i++) - { - node=new COpendriveRoadNode(*object.nodes[i]); - this->nodes.push_back(node); - new_node_ref[object.nodes[i]]=node; - } - for(unsigned int i=0;i<object.segments.size();i++) - { - segment=new COpendriveRoadSegment(*object.segments[i]); - this->segments.push_back(segment); - new_segment_ref[object.segments[i]]=segment; - } - // update references - this->update_references(new_segment_ref,new_lane_ref,new_node_ref); -} - -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(); -} - -void COpendriveRoad::link_segments(OpenDRIVE &open_drive) -{ - std::string predecessor_id,successor_id; - std::string predecessor_contact,successor_contact; - COpendriveRoadSegment *prev_road,*road,*next_road; - std::stringstream error; - - for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it) - { - // get current segment - road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(road_it->id().get()))); - // get predecessor and successor - if(road_it->lane_link().present()) - { - if(road_it->lane_link().get().predecessor().present())// predecessor present - { - if(road_it->lane_link().get().predecessor().get().elementType().get()=="road")// previous segment is a road - { - predecessor_id=road_it->lane_link().get().predecessor().get().elementId().get(); - predecessor_contact=road_it->lane_link().get().predecessor().get().contactPoint().get(); - } - } - if(road_it->lane_link().get().successor().present())// successor present - { - if(road_it->lane_link().get().successor().get().elementType().get()=="road") - { - successor_id=road_it->lane_link().get().successor().get().elementId().get(); - successor_contact=road_it->lane_link().get().successor().get().contactPoint().get(); - } - } - } - if(std::stoi(road_it->junction().get())==-1)// non junction road segments - { - if(!predecessor_id.empty()) - { - prev_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(predecessor_id))); - prev_road->link_segment(road); - predecessor_id.clear(); - } - if(!successor_id.empty()) - { - next_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(successor_id))); - road->link_segment(next_road); - 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) - { - std::string incoming_road_id; - std::string connecting_road_id; - if(connection_it->incomingRoad().present()) - incoming_road_id=connection_it->incomingRoad().get(); - else - { - error << "Connectivity information missing for segment " << road->get_name() << ": No incomming segment"; - throw CException(_HERE_,error.str()); - } - if(connection_it->connectingRoad().present()) - connecting_road_id=connection_it->connectingRoad().get(); - else - { - error << "Connectivity information missing for segment " << road->get_name() << ": No connecting segment"; - throw CException(_HERE_,error.str()); - } - if(predecessor_id.compare(incoming_road_id)==0 && successor_id.compare(connecting_road_id)==0)// this is the connection - { - prev_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(predecessor_id))); - next_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(successor_id))); - 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 - { - error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing from identifier"; - throw CException(_HERE_,error.str()); - } - if(lane_link_it->to().present()) - to_lane_id=lane_link_it->to().get(); - else - { - error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing to identifier"; - throw CException(_HERE_,error.str()); - } - if(predecessor_contact=="end") - prev_road->link_segment(road,from_lane_id,false,-1,true); - else - prev_road->link_segment(road,from_lane_id,true,-1,true); - TOpendriveWorldPose end=road->get_lane(-1).get_end_pose(); - TOpendriveWorldPose start; - if(successor_contact=="end") - { - if(to_lane_id<0) - start=next_road->get_lane(to_lane_id).get_end_pose(); - else - start=next_road->get_lane(to_lane_id).get_start_pose(); - if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution) - road->link_segment(next_road,-1,false,to_lane_id,false); - } - else - { - if(to_lane_id<0) - start=next_road->get_lane(to_lane_id).get_start_pose(); - else - start=next_road->get_lane(to_lane_id).get_end_pose(); - if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution) - road->link_segment(next_road,-1,false,to_lane_id,true); - } - } - } - } - } - } - } -} - -unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node) -{ - std::stringstream error; - - for(unsigned int i=0;i<this->nodes.size();i++) - { - if(this->nodes[i]==node) - { - error << "Node " << node->get_index() << " already present"; - throw CException(_HERE_,error.str()); - } - } - this->nodes.push_back(node); - - return this->nodes.size()-1; -} - -bool COpendriveRoad::remove_node(COpendriveRoadNode *node) -{ - 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); - return true; - } - else - it++; - } - - return false; -} - -unsigned int COpendriveRoad::add_lane(COpendriveLane *lane) -{ - std::stringstream error; - - for(unsigned int i=0;i<this->lanes.size();i++) - { - if(this->lanes[i]==lane) - { - error << "Lane " << lane->get_index() << " already present"; - throw CException(_HERE_,error.str()); - } - } - this->lanes.push_back(lane); - - return this->lanes.size()-1; -} - -bool COpendriveRoad::remove_lane(COpendriveLane *lane) -{ - 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); - return true; - } - else - it++; - } - - return false; -} - -void COpendriveRoad::complete_open_lanes(void) -{ - std::vector<COpendriveLane *>::iterator lane_it; - TOpendriveWorldPose end_pose; - - // 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 - { - try{ - end_pose=(*lane_it)->get_end_pose(); - if(!this->node_exists_at(end_pose))// there is no node at the end pose - { - (*lane_it)->segment->add_empty_node(end_pose,(*lane_it)); - (*lane_it)->segment->link_neightbour_lanes(); - } - else - lane_it++; - }catch(CException &e){ - lane_it++; - } - } - else - lane_it++; - } -} - -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; -} - -void COpendriveRoad::add_segment(COpendriveRoadSegment *segment) -{ - 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]); - // update the road reference - segment->set_parent_road(this); -} - -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; -} - -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();) - { - if((*seg_it)->updated(segment_refs)) - { - (*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();) - { - if(!(*lane_it)->updated(lane_refs)) - lane_it=this->lanes.erase(lane_it); - else - lane_it++; - } - for(node_it=this->nodes.begin();node_it!=this->nodes.end();) - { - if(!(*node_it)->updated(node_refs)) - node_it=this->nodes.erase(node_it); - else - node_it++; - } -} - -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); -} - -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++) - { - if((*lane_it)->has_node_with_index(path_nodes[i])) - { - 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++) - { - if(neightbour_lane->has_node_with_index(path_nodes[i])) - { - 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++) - { - if(neightbour_lane->has_node_with_index(path_nodes[i])) - { - 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++) - { - if(!this->has_segment(this->nodes[i]->links[j]->segment)) - this->nodes[i]->remove_link(this->nodes[i]->links[j]); - } - } -} - -bool COpendriveRoad::node_exists_at(TOpendriveWorldPose &pose) -{ - TOpendriveWorldPose node_pose; - - for(unsigned int i=0;i<nodes.size();i++) - { - node_pose=this->nodes[i]->get_pose(); - if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01) - return true; - } - - return false; -} - -COpendriveRoadNode* COpendriveRoad::get_node_at(TOpendriveWorldPose &pose) -{ - TOpendriveWorldPose node_pose; - - for(unsigned int i=0;i<nodes.size();i++) - { - node_pose=this->nodes[i]->get_pose(); - 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; -} - -void COpendriveRoad::load(const std::string &filename) -{ - struct stat buffer; - - if(stat(filename.c_str(),&buffer)==0) - { - // delete any previous data - this->free(); - // 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) - { - try{ - COpendriveRoadSegment *segment=new COpendriveRoadSegment(); - segment->set_resolution(this->resolution); - segment->set_scale_factor(this->scale_factor); - segment->set_min_road_length(this->min_road_length); - segment->set_parent_road(this); - segment->load(*road_it); - this->segments.push_back(segment); - }catch(CException &e){ - std::cout << e.what() << std::endl; - } - } - // link segments - this->link_segments(*open_drive); - }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); -} - -double COpendriveRoad::get_resolution(void) const -{ - 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); -} - -double COpendriveRoad::get_scale_factor(void) const -{ - return this->scale_factor; -} - -void COpendriveRoad::set_min_road_length(double length) -{ - this->min_road_length=length; - - for(unsigned int i=0;i<this->segments.size();i++) - this->segments[i]->set_min_road_length(length); -} - -double COpendriveRoad::get_min_road_length(void) const -{ - return this->min_road_length; -} - -unsigned int COpendriveRoad::get_num_segments(void) const -{ - return this->segments.size(); -} - -const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) const -{ - std::stringstream error; - - if(index>=0 && index<this->segments.size()) - return *this->segments[index]; - else - { - error << "Invalid segment index " << index; - throw CException(_HERE_,error.str()); - } -} - -const COpendriveRoadSegment &COpendriveRoad::get_segment_by_id(unsigned int id) const -{ - std::stringstream error; - - for(unsigned int i=0;i<this->segments.size();i++) - { - if(this->segments[i]->get_id()==id) - return *this->segments[i]; - } - - error << "No road segment with the " << id << " ID"; - throw CException(_HERE_,error.str()); -} - -const COpendriveRoadSegment &COpendriveRoad::get_segment_by_name(std::string &name) const -{ - std::stringstream error; - - for(unsigned int i=0;i<this->segments.size();i++) - { - if(this->segments[i]->get_name()==name) - return *this->segments[i]; - } - - error << "No road segment with the " << name << " name"; - throw CException(_HERE_,error.str()); -} - -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()); - } -} - -unsigned int COpendriveRoad::get_num_nodes(void) const -{ - return this->nodes.size(); -} - -const COpendriveRoadNode& COpendriveRoad::get_node(unsigned int index) const -{ - std::stringstream error; - - if(index>=0 && index<this->nodes.size()) - return *this->nodes[index]; - else - { - error << "Invalid node index " << index; - throw CException(_HERE_,error.str()); - } -} - -const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index) -{ - std::stringstream error; - - if(index>=0 && index<this->segments.size()) - return *this->segments[index]; - else - { - error << "Invalid node index " << index; - throw CException(_HERE_,error.str()); - } -} - -unsigned int COpendriveRoad::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const -{ - double dist,min_dist=std::numeric_limits<double>::max(),length; - TOpendriveWorldPose closest_pose; - unsigned int closest_index=-1; - - for(unsigned int i=0;i<this->nodes.size();i++) - { - length=this->nodes[i]->get_closest_pose(pose,closest_pose,false,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; - } - } - } - - 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 -{ - double dist,min_dist=std::numeric_limits<double>::max(),length; - TOpendriveWorldPose closest_pose; - unsigned int closest_index=-1; - - for(unsigned int i=0;i<this->lanes.size();i++) - { - 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; - } - } - } - - return closest_index; -} - -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 -{ - double dist,min_dist=std::numeric_limits<double>::max(),length; - TOpendriveWorldPose closest_pose; - unsigned int closest_index=-1; - - for(unsigned int i=0;i<this->segments.size();i++) - { - 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; - } - } - } - - 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 -{ - double dist,min_dist=std::numeric_limits<double>::max(); - TOpendriveWorldPose pose_found; - double length,closest_length; - - for(unsigned int i=0;i<this->nodes.size();i++) - { - length=this->nodes[i]->get_closest_pose(pose,pose_found,false,angle_tol); - if(length<std::numeric_limits<double>::max()) - { - 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; - } - } - } - - return closest_length; -} - -void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &distances,double dist_tol,double angle_tol) const -{ - std::vector<TOpendriveWorldPose> poses; - std::vector<double> lengths; - bool already_added; - - closest_poses.clear(); - distances.clear(); - for(unsigned int i=0;i<this->nodes.size();i++) - { - this->nodes[i]->get_closest_poses(pose,poses,lengths,dist_tol,false,angle_tol); - for(unsigned int j=0;j<poses.size();j++) - { - 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]); - distances.push_back(lengths[j]); - } - } - } -} - -std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road) -{ - segment_up_ref_t new_segment_ref; - lane_up_ref_t new_lane_ref; - node_up_ref_t new_node_ref; - link_up_ref_t new_link_ref; - COpendriveRoadNode *node,*next_node; - COpendriveRoadSegment *segment,*new_segment; -// CopendriveRoadSegment *original_seg1,*original_seg2; - COpendriveLink *link; - std::vector<unsigned int> new_path_nodes; - unsigned int link_index; - double distance; - - new_path_nodes.resize(path_nodes.size()); - - 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); - - for(unsigned int i=0;i<path_nodes.size()-1;i++) - { - 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()) - { - new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref); - new_road.add_segment(new_segment); - new_segment_ref[segment]=new_segment; - } - } - // add the last segment - node=this->nodes[path_nodes[path_nodes.size()-1]]; - link_index=node->get_closest_link_index(end_pose,distance); - if(link_index==(unsigned int)-1) - throw CException(_HERE_,"The provided path has unconnected nodes"); - link=node->links[link_index]; - segment=link->segment; - if(new_segment_ref.find(segment)==new_segment_ref.end()) - { - new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref); - new_road.add_segment(new_segment); - new_segment_ref[segment]=new_segment; - } - - // add additional nodes not explicitly in the path -/* - for(unsigned int i=0;i<this->segments.size();i++) - { - for(unsigned int j=0;j<new_road.segments.size();j++) - { - original_seg1=new_road.segments[j]->get_original_segment(new_segment_ref); - for(unsigned int k=j+1;k<new_road.segments.size();k++) - { - original_seg2=new_road.segments[k]->get_original_segment(new_segment_ref); - if(this->segments[i]->connects_segments(original_seg1,original_seg2) - { - if(!new_road.has_segment(new_segment_ref[this->segments[i]])) - { - new_segment=this->segment[k]->clone(new_node_ref,new_lane_ref,new_link_ref); - new_road.add_segment(new_segment); - new_segment_ref[segment]=new_segment; - } - } - } - } - } -*/ - 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); - // remove unconnected elements - new_road.prune(path_nodes); - new_road.reindex(); - new_road.complete_open_lanes(); - - return new_road.update_path(new_node_ref,path_nodes); -} - -void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length,double margin,COpendriveRoad &new_road) -{ - segment_up_ref_t new_segment_ref; - lane_up_ref_t new_lane_ref; - node_up_ref_t new_node_ref; - link_up_ref_t new_link_ref; - std::vector<const COpendriveRoadSegment *> cand_segments; - std::vector<int> cand_sides; - unsigned int segment_index; - TOpendriveWorldPose end_pose,int_pose; - const COpendriveRoadSegment *segment; - COpendriveRoadSegment *new_segment; - double rem_length=length+2.0*margin,distance,start_length; - int node_side; - - 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); - // get the first segment - segment_index=this->get_closest_segment_index(start_pose,distance); - if(segment_index==(unsigned int)-1) - throw CException(_HERE_,"Start position does not belong to the road"); - segment=this->segments[segment_index]; - node_side=segment->get_pose_side(start_pose); - if(margin>0.0) - { - if(node_side<0) - { - rem_length=distance-margin; - if(rem_length<0.0)// get the previous segment - { - cand_segments=segment->get_prev_segments(node_side,cand_sides); - if(cand_segments.size()>1) - throw CException(_HERE_,"Road has multiple path, impossible to generate new road"); - if(cand_segments.size()==0) - { - int_pose=segment->get_pose_at(0.0); - distance=0.0; - } - else - { - node_side=cand_sides[0]; - segment=cand_segments[0]; - if(node_side<0) - { - int_pose=segment->get_pose_at(segment->get_length()+rem_length); - distance=segment->get_length()+rem_length; - } - else - { - int_pose=segment->get_pose_at(-rem_length); - distance=-rem_length; - } - } - } - else - { - int_pose=segment->get_pose_at(rem_length); - distance-=margin; - } - } - else - { - rem_length=distance+margin-segment->get_length(); - if(rem_length>0.0)// get the prev segment - { - cand_segments=segment->get_prev_segments(node_side,cand_sides); - if(cand_segments.size()>1) - throw CException(_HERE_,"Road has multiple path, impossible to generate new road"); - if(cand_segments.size()==0) - { - int_pose=segment->get_pose_at(segment->get_length()); - distance=0.0; - } - else - { - node_side=cand_sides[0]; - segment=cand_segments[0]; - if(node_side<0) - { - int_pose=segment->get_pose_at(segment->get_length()-rem_length); - distance=segment->get_length()-rem_length; - } - else - { - int_pose=segment->get_pose_at(rem_length); - distance=rem_length; - } - } - } - else - { - int_pose=segment->get_pose_at(distance+margin); - distance+=margin; - } - } - } - else - int_pose=start_pose; - rem_length=length+2.0*margin; - new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,&int_pose,NULL); - if(new_segment->get_length()<this->min_road_length) - { - delete new_segment; - cand_segments=segment->get_next_segments(node_side,cand_sides); - if(cand_segments.size()>1) - throw CException(_HERE_,"Road has multiple path, impossible to generate new road"); - if(cand_segments.size()!=0) - { - node_side=cand_sides[0]; - segment=cand_segments[0]; - new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref); - distance=0.0; - } - else - return; - } - 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); - delete new_segment; - new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,&start_pose,&end_pose); - } - rem_length-=new_segment->get_length(); - new_road.add_segment(new_segment); - new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment; - while(rem_length>this->resolution) - { - cand_segments=segment->get_next_segments(node_side,cand_sides); - if(cand_segments.size()>1) - throw CException(_HERE_,"Road has multiple path, impossible to generate new road"); - if(cand_segments.size()==0) - break; - else - { - node_side=cand_sides[0]; - segment=cand_segments[0]; - if((rem_length-segment->get_length())<0.0) - { - if(node_side<0) - end_pose=segment->get_pose_at(rem_length); - else - end_pose=segment->get_pose_at(segment->get_length()-rem_length); - new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose); - } - else - new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref); - if(new_segment->get_length()>this->min_road_length) - { - rem_length-=new_segment->get_length(); - new_road.add_segment(new_segment); - new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment; - } - else - break; - } - } - 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(); -} - -void COpendriveRoad::operator=(const COpendriveRoad& object) -{ - COpendriveRoadSegment *segment; - COpendriveRoadNode *node; - COpendriveLane *lane; - segment_up_ref_t new_segment_ref; - node_up_ref_t new_node_ref; - lane_up_ref_t new_lane_ref; - - this->free(); - this->resolution=object.resolution; - this->scale_factor=object.scale_factor; - this->min_road_length=object.min_road_length; - 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; - } - - for(unsigned int i=0;i<object.nodes.size();i++) - { - node=new COpendriveRoadNode(*object.nodes[i]); - this->nodes.push_back(node); - new_node_ref[object.nodes[i]]=node; - } - - for(unsigned int i=0;i<object.segments.size();i++) - { - segment=new COpendriveRoadSegment(*object.segments[i]); - this->segments.push_back(segment); - new_segment_ref[object.segments[i]]=segment; - } - // update references - this->update_references(new_segment_ref,new_lane_ref,new_node_ref); -} - -std::ostream& operator<<(std::ostream& out, COpendriveRoad &road) -{ - out << "Resolution: " << road.resolution << std::endl; - out << "Scale factor: " << road.scale_factor << std::endl; - out << "Minimum road lenght: " << road.min_road_length << std::endl; - out << "Total number of nodes: " << road.nodes.size() << std::endl; - for(unsigned int i=0;i<road.segments.size();i++) - std::cout << *road.segments[i]; - - return out; -} - -COpendriveRoad::~COpendriveRoad() -{ - this->free(); - this->resolution=DEFAULT_RESOLUTION; - this->scale_factor=DEFAULT_SCALE_FACTOR; - this->min_road_length=DEFAULT_MIN_ROAD_LENGTH; -} - diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp deleted file mode 100644 index 6551616292dd51f096af8d6e7f13368c5a6cf105..0000000000000000000000000000000000000000 --- a/src/opendrive_road_node.cpp +++ /dev/null @@ -1,614 +0,0 @@ -#include "opendrive_road_node.h" -#include "opendrive_road.h" -#include "exceptions.h" -#include <math.h> - -COpendriveRoadNode::COpendriveRoadNode() -{ - this->resolution=DEFAULT_RESOLUTION; - this->scale_factor=DEFAULT_SCALE_FACTOR; - this->pose.x=0.0; - this->pose.y=0.0; - this->pose.heading=0.0; - this->parents.clear(); - this->links.clear(); - this->index=-1; -} - -COpendriveRoadNode *COpendriveRoadNode::clone(link_up_ref_t &new_link_ref) -{ - COpendriveRoadNode *new_node; - - new_node=new COpendriveRoadNode(); - new_node->resolution=this->resolution; - new_node->scale_factor=this->scale_factor; - new_node->pose=this->pose; - new_node->parents=this->parents; - new_node->links.resize(this->links.size()); - for(unsigned int i=0;i<this->links.size();i++) - { - if(new_link_ref.find(this->links[i])!=new_link_ref.end()) - new_node->links[i]=new_link_ref[this->links[i]]; - else - { - new_node->links[i]=new COpendriveLink(*this->links[i]); - new_link_ref[this->links[i]]=new_node->links[i]; - } - } - new_node->index=this->index; - - return new_node; -} - -bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane) -{ - TOpendriveRoadNodeParent *parent; - TOpendriveWorldPose node_pose; - double start_curvature,end_curvature; - COpendriveLink *new_link; - - if(this->has_link_with(*node) || node->has_link_with(*this)) - return false; - new_link=new COpendriveLink(); - new_link->set_prev(this); - new_link->set_next(node); - new_link->set_resolution(this->resolution); - new_link->set_scale_factor(this->scale_factor); - new_link->set_road_mark(mark); - new_link->set_parent_segment(segment); - new_link->set_parent_lane(lane); - // get the curvature - node_pose=node->get_pose(); - parent=this->get_parent_with_lane(lane); - if(parent!=NULL) - { - start_curvature=parent->start_curvature; - end_curvature=parent->end_curvature; - } - else - { - if(this->get_num_parents()==1) - { - start_curvature=this->parents[0].start_curvature; - end_curvature=this->parents[0].end_curvature; - } - else - { - start_curvature=0.0; - end_curvature=0.0; - } - } - new_link->generate(start_curvature,end_curvature); - this->add_link(new_link); - node->add_link(new_link); - - return true; -} - -void COpendriveRoadNode::add_link(COpendriveLink *link) -{ - if(!this->has_link(link)) - this->links.push_back(link); -} - -void COpendriveRoadNode::remove_link(COpendriveLink *link) -{ - std::vector<COpendriveLink *>::iterator it; - - for(it=this->links.begin();it!=this->links.end();) - { - if((*it)==link) - { - if((*it)->prev==this) - delete (*it); - it=this->links.erase(it); - break; - } - else - it++; - } -} - -bool COpendriveRoadNode::has_link(COpendriveLink *link) -{ - for(unsigned int i=0;i<this->links.size();i++) - if(this->links[i]==link) - return true; - - return false; -} - -bool COpendriveRoadNode::has_link_with(const COpendriveRoadNode &node) const -{ - for(unsigned int i=0;i<this->links.size();i++) - if(this->links[i]->prev==this && this->links[i]->next==&node) - return true; - - return false; -} - -bool COpendriveRoadNode::has_link_with(unsigned int node_index) const -{ - for(unsigned int i=0;i<this->links.size();i++) - if(this->links[i]->prev==this && this->links[i]->next->get_index()==node_index) - return true; - - return false; -} - -COpendriveLink *COpendriveRoadNode::get_link_with(COpendriveRoadNode *node) -{ - for(unsigned int i=0;i<this->links.size();i++) - { - if(this->links[i]->prev==node || this->links[i]->next==node) - return this->links[i]; - } - - return NULL; -} - -void COpendriveRoadNode::set_resolution(double res) -{ - this->resolution=res; - - for(unsigned int i=0;i<this->links.size();i++) - this->links[i]->set_resolution(res); -} - -void COpendriveRoadNode::set_scale_factor(double scale) -{ - this->scale_factor=scale; - - for(unsigned int i=0;i<this->links.size();i++) - this->links[i]->set_scale_factor(scale); - - for(unsigned int i=0;i<this->parents.size();i++) - { - this->parents[i].start_curvature*=scale/this->scale_factor; - this->parents[i].end_curvature*=scale/this->scale_factor; - } -} - -void COpendriveRoadNode::set_index(unsigned int index) -{ - this->index=index; -} - -void COpendriveRoadNode::set_pose(TOpendriveWorldPose &start) -{ - this->pose=start; -} - -void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double start_curvature,double end_curvature) -{ - TOpendriveRoadNodeParent new_parent; - - for(unsigned int i=0;i<this->parents.size();i++) - if(this->parents[i].lane==lane && this->parents[i].segment==segment) - return; - new_parent.lane=lane; - new_parent.segment=segment; - new_parent.start_curvature=start_curvature; - new_parent.end_curvature=end_curvature; - this->parents.push_back(new_parent); -} - -TOpendriveRoadNodeParent *COpendriveRoadNode::get_parent_with_lane(const COpendriveLane *lane) -{ - for(unsigned int i=0;i<this->parents.size();i++) - if(this->parents[i].lane==lane) - return &this->parents[i]; - - return NULL; -} - -TOpendriveRoadNodeParent *COpendriveRoadNode::get_parent_with_segment(const COpendriveRoadSegment *segment) -{ - for(unsigned int i=0;i<this->parents.size();i++) - if(this->parents[i].segment==segment) - return &this->parents[i]; - - return NULL; -} - - -void COpendriveRoadNode::free(void) -{ - this->parents.clear(); - for(unsigned int i=0;i<this->links.size();i++) - { - if(this->links[i]->prev==this) - { - delete this->links[i]; - this->links[i]=NULL; - } - } - this->links.clear(); -} - -bool COpendriveRoadNode::updated(node_up_ref_t &refs) -{ - node_up_ref_t::iterator updated_it; - - for(updated_it=refs.begin();updated_it!=refs.end();updated_it++) - if(updated_it->second==this) - return true; - - return false; -} - -COpendriveRoadNode *COpendriveRoadNode::get_original_node(node_up_ref_t &node_refs) -{ - node_up_ref_t::iterator updated_it; - - for(updated_it=node_refs.begin();updated_it!=node_refs.end();updated_it++) - if(updated_it->second==this) - return updated_it->first; - - return NULL; -} - -void COpendriveRoadNode::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs) -{ - std::vector<COpendriveLink *>::iterator link_it; - - if(this->updated(node_refs)) - { - for(link_it=this->links.begin();link_it!=this->links.end();link_it++) - (*link_it)->update_references(segment_refs,lane_refs,node_refs); - for(unsigned int i=0;i<this->parents.size();i++) - { - if(segment_refs.find(this->parents[i].segment)!=segment_refs.end()) - this->parents[i].segment=segment_refs[this->parents[i].segment]; - if(lane_refs.find(this->parents[i].lane)!=lane_refs.end()) - this->parents[i].lane=lane_refs[this->parents[i].lane]; - } - } -} - -void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs) -{ - std::vector<TOpendriveRoadNodeParent>::iterator parent_it; - std::vector<COpendriveLink *>::iterator link_it; - - if(this->updated(node_refs)) - { - for(link_it=this->links.begin();link_it!=this->links.end();) - { - if((*link_it)->clean_references(node_refs)) - link_it=this->links.erase(link_it); - else - link_it++; - } - for(parent_it=this->parents.begin();parent_it!=this->parents.end();) - { - if(!parent_it->segment->updated(segment_refs) || !parent_it->lane->updated(lane_refs)) - parent_it=this->parents.erase(parent_it); - else - parent_it++; - } - } -} - -void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance) -{ - std::vector<TOpendriveRoadNodeParent>::iterator parent_it; - std::vector<COpendriveLink *>::iterator link_it; - TOpendriveRoadNodeParent *parent; - - this->pose=start; - // remove the references to all lanes and segments except for lane - for(parent_it=this->parents.begin();parent_it!=this->parents.end();) - { - if(parent_it->lane!=lane) - parent_it=this->parents.erase(parent_it); - else - { - parent=&(*parent_it); - parent_it++; - } - } - // update the links - for(link_it=this->links.begin();link_it!=this->links.end();) - { - if((*link_it)->next==this) - { - delete *link_it; - link_it=this->links.erase(link_it); - } - else - { - if((*link_it)->lane==lane) - { - parent->start_curvature=(*link_it)->get_curvature_at(distance); - (*link_it)->update_start_pose(start,parent->start_curvature); - } - else - (*link_it)->update_start_pose(start,0.0); - link_it++; - } - } -} - -void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance) -{ - std::vector<TOpendriveRoadNodeParent>::iterator parent_it; - std::vector<COpendriveLink *>::iterator link_it; - - // remove the references to all lanes and segments except for lane - for(parent_it=this->parents.begin();parent_it!=this->parents.end();) - { - if(parent_it->lane!=lane) - parent_it=this->parents.erase(parent_it); - else - parent_it++; - } - // update the links - for(link_it=this->links.begin();link_it!=this->links.end();) - { - if((*link_it)->prev==this) - { - delete *link_it; - link_it=this->links.erase(link_it); - } - else - link_it++; - } -} - -double COpendriveRoadNode::get_resolution(void) const -{ - return this->resolution; -} - -double COpendriveRoadNode::get_scale_factor(void) const -{ - return this->scale_factor; -} - -unsigned int COpendriveRoadNode::get_index(void) const -{ - return this->index; -} - -unsigned int COpendriveRoadNode::get_num_parents(void) const -{ - return this->parents.size(); -} - -const COpendriveRoadSegment &COpendriveRoadNode::get_parent_segment(unsigned int index) const -{ - std::stringstream text; - - if(index>=0 && index<this->parents.size()) - return *this->parents[index].segment; - else - { - text << "Invalid parent index " << index << " for node " << this->index << " (has " << this->parents.size() << " parents)"; - throw CException(_HERE_,text.str()); - } -} - -const COpendriveLane &COpendriveRoadNode::get_parent_lane(unsigned int index) const -{ - std::stringstream text; - - if(index>=0 && index<this->parents.size()) - return *this->parents[index].lane; - else - { - text << "Invalid parent index " << index << " for node " << this->index << " (has " << this->parents.size() << " parents)"; - throw CException(_HERE_,text.str()); - } -} - -TOpendriveWorldPose COpendriveRoadNode::get_pose(void) const -{ - return this->pose; -} - -unsigned int COpendriveRoadNode::get_num_links(void) const -{ - return this->links.size(); -} - -const COpendriveLink &COpendriveRoadNode::get_link(unsigned int index) const -{ - std::stringstream text; - - if(index>=0 && index<this->links.size()) - return *this->links[index]; - else - { - text << "Invalid link index " << index << " for node " << this->index << " (has " << this->links.size() << " links)"; - throw CException(_HERE_,text.str()); - } -} - -const COpendriveLink &COpendriveRoadNode::get_link_ending_at(unsigned int node_index) const -{ - std::stringstream text; - - for(unsigned int i=0;i<this->links.size();i++) - { - if(this->links[i]->get_next().get_index()==node_index) - return *this->links[i]; - } - - text << "No link in node " << this->index << " ends at node " << node_index; - throw CException(_HERE_,text.str()); -} - -const COpendriveLink &COpendriveRoadNode::get_link_ending_at(const COpendriveRoadNode &node) const -{ - std::stringstream text; - - for(unsigned int i=0;i<this->links.size();i++) - { - if(&this->links[i]->get_next()==&node) - return *this->links[i]; - } - - text << "No link in node " << this->index << " ends at node " << node.get_index(); - throw CException(_HERE_,text.str()); -} - -unsigned int COpendriveRoadNode::get_closest_link_index(TOpendriveWorldPose &pose,double &distance,bool only_lanes,double angle_tol) const -{ - double dist,min_dist=std::numeric_limits<double>::max(); - unsigned int closest_index=-1; - double angle,length,min_length=std::numeric_limits<double>::max(); - TOpendriveWorldPose closest_pose; - - for(unsigned int i=0;i<this->links.size();i++) - { - if(&this->links[i]->get_prev()==this)// links starting at this node - { - if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL)) - { - length=this->links[i]->find_closest_world_pose(pose,closest_pose); - if(length<std::numeric_limits<double>::max()) - { - angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading); - if(fabs(angle)<angle_tol) - { - 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; - min_length=length; - closest_index=i; - } - } - } - } - } - } - - distance=min_length; - return closest_index; -} - -const COpendriveLink &COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,double &distance,bool only_lanes,double angle_tol)const -{ - unsigned int closest_index; - - closest_index=this->get_closest_link_index(pose,distance,only_lanes,angle_tol); - if(closest_index==(unsigned int)-1) - throw CException(_HERE_,"Impossible to find the closest link"); - return *this->links[closest_index]; -} - -double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes,double angle_tol) const -{ - double dist,min_dist=std::numeric_limits<double>::max(); - double angle; - double length,closest_length=std::numeric_limits<double>::max(); - TOpendriveWorldPose tmp_pose; - - closest_pose.x=std::numeric_limits<double>::max(); - closest_pose.y=std::numeric_limits<double>::max(); - closest_pose.heading=std::numeric_limits<double>::max(); - for(unsigned int i=0;i<this->links.size();i++) - { - if(&this->links[i]->get_prev()==this)// links starting at this node - { - if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL)) - { - length=this->links[i]->find_closest_world_pose(pose,tmp_pose); - if(length<std::numeric_limits<double>::max()) - { - angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading); - if(fabs(angle)<angle_tol) - { - dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0)); - if(dist<min_dist) - { - min_dist=dist; - closest_length=length; - closest_pose=tmp_pose; - } - } - } - } - } - } - - return closest_length; -} - -void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &distances,double dist_tol,bool only_lanes,double angle_tol) const -{ - double dist; - double angle; - double length; - TOpendriveWorldPose closest_pose; - bool already_added; - - closest_poses.clear(); - distances.clear(); - for(unsigned int i=0;i<this->links.size();i++) - { - if(&this->links[i]->get_prev()==this)// links starting at this node - { - if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL)) - { - length=this->links[i]->find_closest_world_pose(pose,closest_pose); - if(length<std::numeric_limits<double>::max()) - { - angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading); - if(fabs(angle)<angle_tol) - { - dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0)); - if(dist<=dist_tol) - { - already_added=false; - for(unsigned int j=0;j<closest_poses.size();j++) - if(fabs(closest_poses[j].x-closest_pose.x)<this->resolution && - fabs(closest_poses[j].y-closest_pose.y)<this->resolution) - { - already_added=true; - break; - } - if(!already_added) - { - closest_poses.push_back(closest_pose); - distances.push_back(length); - } - } - } - } - } - } - } -} - -std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node) -{ - out << " Node: " << node.get_index() << std::endl; - out << " start pose: x: " << node.pose.x << " y: " << node.pose.y << " heading: " << node.pose.heading << std::endl; - out << " Number of parent: " << node.get_num_parents() << std::endl; - for(unsigned int i=0;i<node.get_num_parents();i++) - { - out << " Parent " << i << ":" << std::endl; - out << " segment " << node.get_parent_segment(i).get_name() << " (lane " << node.get_parent_lane(i).get_id() << ")" << std::endl; - } - out << " Number of links: " << node.links.size() << std::endl; - for(unsigned int i=0;i<node.links.size();i++) - { - out << " Link " << i << ":" << std::endl; - out << *node.links[i]; - } - return out; -} - -COpendriveRoadNode::~COpendriveRoadNode() -{ - this->resolution=DEFAULT_RESOLUTION; - this->scale_factor=DEFAULT_SCALE_FACTOR; - this->pose.x=0.0; - this->pose.y=0.0; - this->pose.heading=0.0; - this->free(); -} - diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp deleted file mode 100644 index bfd6b041e88d7cf8a39a289c190883adafcd6f79..0000000000000000000000000000000000000000 --- a/src/opendrive_road_segment.cpp +++ /dev/null @@ -1,1336 +0,0 @@ -#include "opendrive_road_segment.h" -#include "exceptions.h" -#include <math.h> - -COpendriveRoadSegment::COpendriveRoadSegment() -{ - this->name=""; - this->id=-1; - this->center_mark=OD_MARK_NONE; - this->lanes.clear(); - this->num_left_lanes=0; - this->num_right_lanes=0; - this->nodes.clear(); - this->signals.clear(); - this->objects.clear(); - this->connecting.clear(); - this->min_road_length=DEFAULT_MIN_ROAD_LENGTH; - this->resolution=DEFAULT_RESOLUTION; - this->scale_factor=DEFAULT_SCALE_FACTOR; - this->geometries.clear(); -} - -COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object) -{ - this->name=object.name; - this->id=object.id; - this->lanes=object.lanes; - this->num_right_lanes=object.num_right_lanes; - this->num_left_lanes=object.num_left_lanes; - this->nodes=object.nodes; - this->parent_road=object.parent_road; - this->signals.clear(); - this->signals.resize(object.signals.size()); - for(unsigned int i=0;i<object.signals.size();i++) - this->signals[i]=new COpendriveSignal(*object.signals[i]); - this->objects.resize(object.objects.size()); - for(unsigned int i=0;i<object.signals.size();i++) - this->objects[i]=new COpendriveObject(*object.objects[i]); - this->connecting=object.connecting; - this->resolution=object.resolution; - this->scale_factor=object.scale_factor; - this->min_road_length=object.min_road_length; - this->center_mark=object.center_mark; - this->geometries.resize(object.geometries.size()); - for(unsigned int i=0;i<object.geometries.size();i++) - { - this->geometries[i].opendrive=object.geometries[i].opendrive->clone(); - this->geometries[i].spline=new CG2Spline(*object.geometries[i].spline); - } -} - -void COpendriveRoadSegment::free(void) -{ - this->lanes.clear(); - this->num_left_lanes=0; - this->num_right_lanes=0; - for(unsigned int i=0;i<this->signals.size();i++) - { - delete this->signals[i]; - this->signals[i]=NULL; - } - this->nodes.clear(); - this->signals.clear(); - for(unsigned int i=0;i<this->objects.size();i++) - { - delete this->objects[i]; - this->objects[i]=NULL; - } - this->objects.clear(); - this->connecting.clear(); - for(unsigned int i=0;i<this->geometries.size();i++) - { - delete this->geometries[i].opendrive; - this->geometries[i].opendrive=NULL; - delete this->geometries[i].spline; - this->geometries[i].spline=NULL; - } - this->geometries.clear(); -} - -void COpendriveRoadSegment::set_resolution(double res) -{ - std::map<int,COpendriveLane *>::const_iterator lane_it; - - this->resolution=res; - - for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it) - lane_it->second->set_resolution(res); - - for(unsigned int i=0;i<this->geometries.size();i++) - this->geometries[i].spline->generate(res); -} - -void COpendriveRoadSegment::set_scale_factor(double scale) -{ - std::map<int,COpendriveLane *>::const_iterator lane_it; - TPoint spline_start,spline_end; - - this->scale_factor=scale; - - for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it) - lane_it->second->set_scale_factor(scale); - for(unsigned int i=0;i<this->signals.size();i++) - this->signals[i]->set_scale_factor(scale); - for(unsigned int i=0;i<this->objects.size();i++) - this->objects[i]->set_scale_factor(scale); - - for(unsigned int i=0;i<this->geometries.size();i++) - { - this->geometries[i].opendrive->set_scale_factor(scale); - this->geometries[i].spline->get_start_point(spline_start); - spline_start.x*=this->scale_factor/scale; - spline_start.y*=this->scale_factor/scale; - spline_start.curvature*=scale/this->scale_factor; - this->geometries[i].spline->set_start_point(spline_start); - this->geometries[i].spline->get_end_point(spline_end); - spline_end.x*=this->scale_factor/scale; - spline_end.y*=this->scale_factor/scale; - spline_end.curvature*=scale/this->scale_factor; - this->geometries[i].spline->set_end_point(spline_end); - this->geometries[i].spline->generate(this->resolution); - } -} - -void COpendriveRoadSegment::set_min_road_length(double length) -{ - this->min_road_length=length; -} - -void COpendriveRoadSegment::set_parent_road(COpendriveRoad *parent) -{ - this->parent_road=parent; -} - -void COpendriveRoadSegment::set_name(std::string &name) -{ - this->name=name; -} - -void COpendriveRoadSegment::set_id(unsigned int id) -{ - this->id=id; -} - -void COpendriveRoadSegment::set_center_mark(opendrive_mark_t mark) -{ - this->center_mark=mark; -} - -void COpendriveRoadSegment::add_geometries(OpenDRIVE::road_type &road_info) -{ - planView::geometry_iterator geom_info; - TOpendriveRoadSegmentGeometry geometry; - TOpendriveWorldPose start_pose,end_pose; - TPoint spline_start,spline_end; - double start_curvature,end_curvature; - std::stringstream error; - - for(unsigned int i=0;i<this->geometries.size();i++) - { - delete this->geometries[i].opendrive; - delete this->geometries[i].spline; - } - this->geometries.clear(); - for(geom_info=road_info.planView().geometry().begin(); geom_info!=road_info.planView().geometry().end(); ++geom_info) - { - // import geometry - if(geom_info->line().present()) - geometry.opendrive=new COpendriveLine(); - else if(geom_info->spiral().present()) - geometry.opendrive=new COpendriveSpiral(); - else if(geom_info->arc().present()) - geometry.opendrive=new COpendriveArc(); - else if(geom_info->paramPoly3().present()) - geometry.opendrive=new COpendriveParamPoly3(); - else - { - error << "Unsupported or missing geometry in road " << this->name; - throw CException(_HERE_,error.str()); - } - geometry.opendrive->set_scale_factor(this->scale_factor); - geometry.opendrive->load(*geom_info); - if(geometry.opendrive->get_length()>this->min_road_length) - { - // create spline - geometry.spline=new CG2Spline(); - start_pose=geometry.opendrive->get_start_pose(); - end_pose=geometry.opendrive->get_end_pose(); - geometry.opendrive->get_curvature(start_curvature,end_curvature); - spline_start.x=start_pose.x; - spline_start.y=start_pose.y; - spline_start.heading=start_pose.heading; - spline_start.curvature=start_curvature; - geometry.spline->set_start_point(spline_start); - spline_end.x=end_pose.x; - spline_end.y=end_pose.y; - spline_end.heading=end_pose.heading; - spline_end.curvature=end_curvature; - geometry.spline->set_end_point(spline_end); - geometry.spline->generate(this->resolution,geometry.opendrive->get_length()); - this->geometries.push_back(geometry); - } - } -} - -bool COpendriveRoadSegment::updated(segment_up_ref_t &refs) -{ - segment_up_ref_t::iterator updated_it; - - for(updated_it=refs.begin();updated_it!=refs.end();updated_it++) - if(updated_it->second==this) - return true; - - return false; -} - -COpendriveRoadSegment *COpendriveRoadSegment::get_original_segment(segment_up_ref_t &segment_refs) -{ - segment_up_ref_t::iterator updated_it; - - for(updated_it=segment_refs.begin();updated_it!=segment_refs.end();updated_it++) - if(updated_it->second==this) - return updated_it->first; - - return NULL; -} - -void COpendriveRoadSegment::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::map<int,COpendriveLane *>::iterator lane_it; - std::vector<COpendriveRoadNode *>::iterator node_it; - - if(this->updated(segment_refs)) - { - for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++) - { - if(lane_refs.find((*lane_it).second)!=lane_refs.end()) - { - lane_it->second=lane_refs[lane_it->second]; - lane_it->second->update_references(segment_refs,lane_refs,node_refs); - } - else if(lane_it->second->updated(lane_refs)) - lane_it->second->update_references(segment_refs,lane_refs,node_refs); - } - 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]; - for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();seg_it++) - if(segment_refs.find(*seg_it)!=segment_refs.end()) - (*seg_it)=segment_refs[*seg_it]; - for(unsigned int i=0;i<this->signals.size();i++) - this->signals[i]->update_references(segment_refs); - for(unsigned int i=0;i<this->objects.size();i++) - this->objects[i]->update_references(segment_refs); - } -} - -void COpendriveRoadSegment::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::map<int,COpendriveLane *>::iterator lane_it; - std::vector<COpendriveRoadNode *>::iterator node_it; - - if(this->updated(segment_refs)) - { - for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();) - { - if(lane_it->second->updated(lane_refs)) - { - lane_it->second->clean_references(segment_refs,lane_refs,node_refs); - lane_it++; - } - else - lane_it=this->lanes.erase(lane_it); - } - for(node_it=this->nodes.begin();node_it!=this->nodes.end();) - { - if(!(*node_it)->updated(node_refs)) - node_it=this->nodes.erase(node_it); - else - node_it++; - } - for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();) - { - if(!(*seg_it)->updated(segment_refs)) - seg_it=this->connecting.erase(seg_it); - else - seg_it++; - } - } -} - -void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section) -{ - right::lane_iterator r_lane_it; - left::lane_iterator l_lane_it; - - // right lanes - if(lane_section.right().present()) - { - for(r_lane_it=lane_section.right()->lane().begin();r_lane_it!=lane_section.right()->lane().end();r_lane_it++) - { - this->add_lane(*r_lane_it); - this->num_right_lanes++; - } - } - // left lanes - if(lane_section.left().present()) - { - for(l_lane_it=lane_section.left()->lane().begin();l_lane_it!=lane_section.left()->lane().end();l_lane_it++) - { - this->add_lane(*l_lane_it); - this->num_left_lanes++; - } - } -} - -void COpendriveRoadSegment::add_lane(const ::lane &lane_info) -{ - std::map<int,COpendriveLane *>::iterator lane_it; - COpendriveLane *new_lane; - unsigned int lane_index; - bool exist=false; - - try{ - new_lane=new COpendriveLane(); - new_lane->set_resolution(this->resolution); - new_lane->set_scale_factor(this->scale_factor); - new_lane->load(lane_info,this); - for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++) - if(lane_it->first==new_lane->get_id()) - { - exist=true; - break; - } - if(!exist) - this->lanes[new_lane->get_id()]=new_lane; - lane_index=parent_road->add_lane(new_lane); - new_lane->set_index(lane_index); - }catch(CException &e){ - this->free(); - if(!this->parent_road->remove_lane(new_lane)) - delete new_lane; - throw e; - } -} - -void COpendriveRoadSegment::remove_lane(COpendriveLane *lane) -{ - std::map<int,COpendriveLane *>::iterator lane_it; - std::vector<COpendriveLane *>::iterator other_lane_it; - - for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();) - { - if(lane_it->second==lane) - { - if(lane_it->first>0) - this->num_left_lanes--; - else - this->num_right_lanes--; - // remove associated nodes - for(unsigned int i=0;i<lane->nodes.size();i++) - this->remove_node(lane->nodes[i]); - // remove the reference from neightbour lanes - if(lane_it->second->left_lane!=NULL) - lane_it->second->left_lane->remove_lane(lane); - if(lane_it->second->right_lane!=NULL) - lane_it->second->right_lane->remove_lane(lane); - // remove the reference from next lanes - for(unsigned int i=0;i<lane_it->second->next.size();i++) - lane_it->second->next[i]->remove_lane(lane); - // remove the reference from prev lanes - for(unsigned int i=0;i<lane_it->second->prev.size();i++) - lane_it->second->prev[i]->remove_lane(lane); - lane_it=this->lanes.erase(lane_it); - break; - } - else - lane_it++; - } -} - -void COpendriveRoadSegment::add_nodes(void) -{ - double lane_offset; - - for(unsigned int j=0;j<this->geometries.size();j++) - { - lane_offset=0.0; - for(int i=-1;i>=-this->num_right_lanes;i--) - { - this->lanes[i]->set_offset(lane_offset); - this->add_node(this->geometries[j],this->lanes[i]); - lane_offset-=this->lanes[i]->width; - } - } - for(unsigned int j=0;j<this->geometries.size();j++) - { - lane_offset=0.0; - for(int i=1;i<=this->num_left_lanes;i++) - { - this->lanes[i]->set_offset(lane_offset); - this->add_node(this->geometries[this->geometries.size()-1-j],this->lanes[i]); - lane_offset+=this->lanes[i]->width; - } - } -} - -void COpendriveRoadSegment::add_node(TOpendriveRoadSegmentGeometry &geometry,COpendriveLane *lane) -{ - TOpendriveTrackPose track_pose; - double start_curvature,end_curvature; - COpendriveRoadNode *new_node; - TOpendriveWorldPose node_pose; - unsigned int node_index; - bool exist=false; - - try{ - new_node=new COpendriveRoadNode(); - new_node->set_resolution(this->resolution); - new_node->set_scale_factor(this->scale_factor); - // import start position - track_pose.t=lane->get_center_offset(); - track_pose.heading=0.0; - if(lane->get_id()<0) - { - track_pose.s=0.0; - geometry.opendrive->get_world_pose(track_pose,node_pose); - } - else - { - track_pose.s=geometry.opendrive->get_length(); - geometry.opendrive->get_world_pose(track_pose,node_pose); - node_pose.heading=normalize_angle(node_pose.heading+3.14159); - } - new_node->set_pose(node_pose); - if(this->parent_road->node_exists_at(node_pose)) - { - delete new_node; - new_node=this->parent_road->get_node_at(node_pose); - } - else - { - node_index=this->parent_road->add_node(new_node); - new_node->set_index(node_index); - } - geometry.opendrive->get_curvature(start_curvature,end_curvature); - if(start_curvature>0) - start_curvature=1.0/((1.0/start_curvature)-lane->get_center_offset()); - else - start_curvature=1.0/((1.0/start_curvature)+lane->get_center_offset()); - if(end_curvature>0) - end_curvature=1.0/((1.0/end_curvature)-lane->get_center_offset()); - else - end_curvature=1.0/((1.0/end_curvature)+lane->get_center_offset()); - if(lane->get_id()<0) - new_node->add_parent(lane,this,start_curvature,end_curvature); - else - new_node->add_parent(lane,this,-end_curvature,-start_curvature); - for(unsigned int i=0;i<this->nodes.size();i++) - if(this->nodes[i]==new_node) - { - exist=true; - break; - } - if(!exist) - this->nodes.push_back(new_node); - lane->add_node(new_node); - }catch(CException &e){ - this->free(); - if(!this->parent_road->remove_node(new_node)) - delete new_node; - throw e; - } -} - -void COpendriveRoadSegment::add_empty_node(TOpendriveWorldPose &pose,COpendriveLane *lane) -{ - COpendriveRoadNode *new_node; - unsigned int node_index; - bool exist=false; - - try{ - new_node=new COpendriveRoadNode(); - new_node->set_resolution(this->resolution); - new_node->set_scale_factor(this->scale_factor); - new_node->set_pose(pose); - node_index=this->parent_road->add_node(new_node); - new_node->set_index(node_index); - for(unsigned int i=0;i<this->nodes.size();i++) - if(this->nodes[i]==new_node) - { - exist=true; - break; - } - if(!exist) - this->nodes.push_back(new_node); - lane->add_node(new_node); - }catch(CException &e){ - this->free(); - if(!this->parent_road->remove_node(new_node)) - delete new_node; - throw e; - } -} - -void COpendriveRoadSegment::remove_node(COpendriveRoadNode *node) -{ - std::vector<COpendriveRoadNode *>::iterator it; - - for(it=this->nodes.begin();it!=this->nodes.end();) - { - if((*it)==node) - { - it=this->nodes.erase(it); - break; - } - else - it++; - } -} - -bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node) const -{ - for(unsigned int i=0;i<this->nodes.size();i++) - if(this->nodes[i]==node) - return true; - - return false; -} - -int COpendriveRoadSegment::get_pose_side(TOpendriveWorldPose &pose) const -{ - const COpendriveRoadNode *node; - std::map<int,COpendriveLane *>::const_iterator it; - std::stringstream error; - double distance; - - node=&this->get_closest_node(pose,distance,3.14159); - for(it=this->lanes.begin();it!=this->lanes.end();it++) - if(it->second->has_node((COpendriveRoadNode *)node)) - return it->first; - - error << "Segment " << this->name << " does not include node " << node->get_index(); - throw CException(_HERE_,error.str()); -} - -void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section) -{ - center::lane_type center_lane; - - if(lane_section.center().lane().present()) - { - center_lane=lane_section.center().lane().get(); - if(center_lane.roadMark().size()>1) - throw CException(_HERE_,"Only one road mark supported for lane"); - else if(center_lane.roadMark().size()==0) - this->set_center_mark(OD_MARK_NONE); - else if(center_lane.roadMark().size()==1) - { - if(center_lane.roadMark().begin()->type1().present()) - { - if(center_lane.roadMark().begin()->type1().get()=="none") - this->set_center_mark(OD_MARK_NONE); - else if(center_lane.roadMark().begin()->type1().get()=="solid") - this->set_center_mark(OD_MARK_SOLID); - else if(center_lane.roadMark().begin()->type1().get()=="broken") - this->set_center_mark(OD_MARK_BROKEN); - else if(center_lane.roadMark().begin()->type1().get()=="solid solid") - this->set_center_mark(OD_MARK_SOLID_SOLID); - else if(center_lane.roadMark().begin()->type1().get()=="solid broken") - this->set_center_mark(OD_MARK_SOLID_BROKEN); - else if(center_lane.roadMark().begin()->type1().get()=="broken solid") - this->set_center_mark(OD_MARK_BROKEN_SOLID); - else if(center_lane.roadMark().begin()->type1().get()=="broken broken") - this->set_center_mark(OD_MARK_BROKEN_BROKEN); - else - this->set_center_mark(OD_MARK_NONE); - } - else - this->set_center_mark(OD_MARK_NONE); - } - } - this->link_neightbour_lanes(); -} - -void COpendriveRoadSegment::link_neightbour_lanes(void) -{ - for(int i=-this->num_right_lanes;i<0;i++) - { - if(this->lanes.find(i+1)!=this->lanes.end())// if the lane exists - this->lanes[i]->link_neightbour_lane(this->lanes[i+1]); - else - this->lanes[i]->set_left_lane(NULL,this->center_mark); - } - for(int i=this->num_left_lanes;i>0;i--) - { - if(this->lanes.find(i-1)!=this->lanes.end())// if the lane exists - this->lanes[i]->link_neightbour_lane(this->lanes[i-1]); - else - this->lanes[i]->set_left_lane(NULL,this->center_mark); - } -} - -void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment) -{ - if(this->connects_to(segment) || segment->connects_to(this)) - return; - this->connecting.push_back(segment); - segment->connecting.push_back(this); - // link lanes - for(int i=-this->num_right_lanes;i<0;i++) - { - for(int j=i-1;j<=i+1;j++) - { - if(segment->lanes.find(j)!=segment->lanes.end()) - { - if(j==i-1) - this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->right_mark,false,true,false); - else if(j==i) - this->lanes[i]->link_lane(segment->lanes[j],OD_MARK_NONE,false,true,true); - else - this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->left_mark,false,true,false); - } - } - } - for(int i=1;i<=segment->num_left_lanes;i++) - { - for(int j=i-1;j<=i+1;j++) - { - if(this->lanes.find(j)!=this->lanes.end()) - { - if(j==i-1) - segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->right_mark,false,true,false); - else if(j==i) - segment->lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true,true); - else - segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->left_mark,false,true,false); - } - } - } -} - -void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment,int from,bool from_start,int to,bool to_start) -{ - if(!this->connects_to(segment)) - this->connecting.push_back(segment); - if(!segment->connects_to(this)) - segment->connecting.push_back(this); - // link lanes - if(segment->lanes.find(to)!=segment->lanes.end()) - { - if(this->lanes.find(from-1)!=this->lanes.end()) - this->lanes[from-1]->link_lane(segment->lanes[to],this->lanes[from-1]->right_mark,from_start,to_start,false); - if(this->lanes.find(from)!=this->lanes.end()) - this->lanes[from]->link_lane(segment->lanes[to],OD_MARK_NONE,from_start,to_start,true); - if(this->lanes.find(from+1)!=this->lanes.end()) - this->lanes[from+1]->link_lane(segment->lanes[to],this->lanes[from+1]->left_mark,from_start,to_start,false); - } -/* - if(segment.lanes.find(to-1)!=segment.lanes.end()) - if(this->lanes.find(from)!=this->lanes.end()) - this->lanes[from]->link_lane(segment.lanes[to-1],this->lanes[from]->right_mark,from_start,to_start,false); - if(segment.lanes.find(to+1)!=segment.lanes.end()) - if(this->lanes.find(from)!=this->lanes.end()) - this->lanes[from]->link_lane(segment.lanes[to+1],this->lanes[from]->left_mark,from_start,to_start,false); -*/ -} - -bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment) -{ - for(unsigned int i=0;i<this->connecting.size();i++) - if(this->connecting[i]->get_id()==segment->get_id()) - return true; - - return false; -} - -bool COpendriveRoadSegment::connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2) -{ - if(this->connects_to(segment1) && this->connects_to(segment2)) - return true; - else - return false; -} - -COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start,TOpendriveWorldPose *end) const -{ - TOpendriveWorldPose *start_pose,*end_pose; - std::map<int,COpendriveLane *>::iterator lane_it; - lane_up_ref_t::iterator lane_ref_it; - COpendriveLane *new_lane; - std::vector<COpendriveRoadSegment *>::iterator seg_it; - segment_up_ref_t new_segment_ref; - COpendriveRoadSegment *new_segment; - std::vector<COpendriveRoadNode *>::iterator node_it; - node_up_ref_t::iterator node_ref_it; - std::vector<TOpendriveRoadSegmentGeometry>::iterator geom_it; - TPoint new_point; - double length; - - if(start==NULL && end==NULL) - return this->clone(new_node_ref,new_lane_ref,new_link_ref); - new_segment=new COpendriveRoadSegment(*this); - new_segment_ref[(COpendriveRoadSegment *)this]=new_segment; - if(node_side<0) - { - start_pose=start; - end_pose=end; - } - else - { - start_pose=end; - end_pose=start; - } - /* get the sublanes */ - for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++) - { - new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,new_link_ref,start_pose,end_pose); - new_lane_ref[lane_it->second]=new_lane; - } - new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref); - // update geometry - if(start_pose!=NULL) - { - for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();) - { - length=geom_it->spline->find_closest_point(start_pose->x,start_pose->y,new_point); - if(length<std::numeric_limits<double>::max()) - { - geom_it->spline->set_start_point(new_point); - geom_it->spline->generate(this->resolution); - geom_it->opendrive->set_start_pose(*start_pose); - geom_it->opendrive->set_max_s(geom_it->opendrive->get_max_s()-length); - break; - } - else - geom_it=new_segment->geometries.erase(geom_it); - } - for(unsigned int i=1;i<new_segment->geometries.size();i++) - { - new_segment->geometries[i].opendrive->set_min_s(new_segment->geometries[i].opendrive->get_min_s()-length); - new_segment->geometries[i].opendrive->set_max_s(new_segment->geometries[i].opendrive->get_max_s()-length); - } - } - if(end_pose!=NULL) - { - for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();) - { - length=geom_it->spline->find_closest_point(end_pose->x,end_pose->y,new_point); - if(length<std::numeric_limits<double>::max()) - { - geom_it->spline->set_end_point(new_point); - geom_it->spline->generate(this->resolution); - geom_it->opendrive->set_max_s(geom_it->opendrive->get_min_s()+length);; - geom_it=new_segment->geometries.erase(++geom_it,new_segment->geometries.end()); - break; - } - else - geom_it++; - } - } - - return new_segment; -} - -COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref) const -{ - std::map<int,COpendriveLane *>::iterator lane_it; - lane_up_ref_t::iterator lane_ref_it; - COpendriveLane *new_lane; - segment_up_ref_t new_segment_ref; - COpendriveRoadSegment *new_segment; - std::vector<COpendriveRoadNode *>::iterator node_it; - node_up_ref_t::iterator node_ref_it; - - new_segment=new COpendriveRoadSegment(*this); - new_segment_ref[(COpendriveRoadSegment *)this]=new_segment; - /* get the sublanes */ - for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++) - { - new_lane=lane_it->second->clone(new_node_ref,new_lane_ref,new_link_ref); - new_lane_ref[lane_it->second]=new_lane; - } - new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref); - - return new_segment; -} - -std::vector<const COpendriveRoadSegment *> COpendriveRoadSegment::get_next_segments(int input_side,std::vector<int> &output_sides) const -{ - std::vector<const COpendriveRoadSegment *> segment_candidates; - bool already_present; - - output_sides.clear(); - if(input_side<0) - { - for(unsigned int i=1;i<=this->get_num_right_lanes();i++) - { - const COpendriveLane &lane=this->get_lane(-i); - for(unsigned int j=0;j<lane.get_num_next_lanes();j++) - { - already_present=false; - for(unsigned int k=0;k<segment_candidates.size();k++) - if(segment_candidates[k]==&lane.get_next_lane(j).get_segment()) - { - already_present=true; - break; - } - if(!already_present) - { - segment_candidates.push_back(&lane.get_next_lane(j).get_segment()); - output_sides.push_back(lane.get_next_lane(j).get_id()); - } - } - } - } - else - { - for(unsigned int i=1;i<=this->get_num_left_lanes();i++) - { - const COpendriveLane &lane=this->get_lane(i); - for(unsigned int j=0;j<lane.get_num_next_lanes();j++) - { - already_present=false; - for(unsigned int k=0;k<segment_candidates.size();k++) - if(segment_candidates[k]==&lane.get_next_lane(j).get_segment()) - { - already_present=true; - break; - } - if(!already_present) - { - segment_candidates.push_back(&lane.get_next_lane(j).get_segment()); - output_sides.push_back(lane.get_next_lane(j).get_id()); - } - } - } - } - - return segment_candidates; -} - -std::vector<const COpendriveRoadSegment *> COpendriveRoadSegment::get_prev_segments(int input_side,std::vector<int> &output_sides) const -{ - std::vector<const COpendriveRoadSegment *> segment_candidates; - bool already_present; - - output_sides.clear(); - if(input_side<0) - { - for(unsigned int i=1;i<=this->get_num_right_lanes();i++) - { - const COpendriveLane &lane=this->get_lane(-i); - for(unsigned int j=0;j<lane.get_num_prev_lanes();j++) - { - already_present=false; - for(unsigned int k=0;k<segment_candidates.size();k++) - if(segment_candidates[k]==&lane.get_prev_lane(j).get_segment()) - { - already_present=true; - break; - } - if(!already_present) - { - segment_candidates.push_back(&lane.get_prev_lane(j).get_segment()); - output_sides.push_back(lane.get_prev_lane(j).get_id()); - } - } - } - } - else - { - for(unsigned int i=1;i<=this->get_num_left_lanes();i++) - { - const COpendriveLane &lane=this->get_lane(i); - for(unsigned int j=0;j<lane.get_num_prev_lanes();j++) - { - already_present=false; - for(unsigned int k=0;k<segment_candidates.size();k++) - if(segment_candidates[k]==&lane.get_prev_lane(j).get_segment()) - { - already_present=true; - break; - } - if(!already_present) - { - segment_candidates.push_back(&lane.get_prev_lane(j).get_segment()); - output_sides.push_back(lane.get_prev_lane(j).get_id()); - } - } - } - } - - return segment_candidates; -} - - -void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info) -{ - std::map<int,COpendriveLane *>::const_iterator lane_it; - signals::signal_iterator signal_it; - objects::object_iterator object_it; - lanes::laneSection_iterator lane_section; - COpendriveSignal *new_signal; - COpendriveObject *new_object; - std::stringstream error; - - this->free(); - this->set_name(road_info.name().get()); - this->set_id(std::stoi(road_info.id().get())); - // only one lane section is supported - if(road_info.lanes().laneSection().size()<1) - { - error << "No lane sections defined for segment " << this->name; - throw CException(_HERE_,error.str()); - } - else if(road_info.lanes().laneSection().size()>1) - { - error << "Segment " << this->name << " has more than one lane section"; - throw CException(_HERE_,error.str()); - } - else - lane_section=road_info.lanes().laneSection().begin(); - - try{ - // add lanes - this->add_lanes(*lane_section); - // add geometries - this->add_geometries(road_info); - // add road nodes - this->add_nodes(); - // link lanes - this->link_neightbour_lanes(*lane_section); - // add road signals - if(road_info.signals().present()) - { - for(signal_it=road_info.signals().get().signal().begin();signal_it!=road_info.signals().get().signal().end();++signal_it) - { - new_signal=new COpendriveSignal(); - new_signal->load(*signal_it,this); - new_signal->set_scale_factor(this->scale_factor); - this->signals.push_back(new_signal); - } - } - // add road objects - if(road_info.objects().present()) - { - for(object_it=road_info.objects().get().object().begin();object_it!=road_info.objects().get().object().end();++object_it) - { - new_object=new COpendriveObject(); - new_object->load(*object_it,this); - new_object->set_scale_factor(this->scale_factor); - this->objects.push_back(new_object); - } - } - }catch(CException &e){ - this->free(); - throw e; - } -} - -std::string COpendriveRoadSegment::get_name(void) const -{ - return this->name; -} - -unsigned int COpendriveRoadSegment::get_id(void) const -{ - return this->id; -} - -unsigned int COpendriveRoadSegment::get_num_right_lanes(void) const -{ - return this->num_right_lanes; -} - -unsigned int COpendriveRoadSegment::get_num_left_lanes(void) const -{ - return this->num_left_lanes; -} - -const COpendriveLane &COpendriveRoadSegment::get_lane(int id) const -{ - std::map<int,COpendriveLane *>::const_iterator lane_it; - std::stringstream error; - - for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it) - { - if(lane_it->second->get_id()==id) - return *lane_it->second; - } - - error << "Invalid lane id (" << id << ") for segment " << this->name; - throw CException(_HERE_,error.str()); -} - -unsigned int COpendriveRoadSegment::get_num_nodes(void) const -{ - return this->nodes.size(); -} - -const COpendriveRoadNode &COpendriveRoadSegment::get_node(unsigned int index) const -{ - std::stringstream error; - - if(index>=0 && index<this->nodes.size()) - return *this->nodes[index]; - else - { - error << "Invalid node index (" << index << ") for segment " << this->name; - throw CException(_HERE_,error.str()); - } -} - -const COpendriveRoad &COpendriveRoadSegment::get_parent_road(void) const -{ - return *this->parent_road; -} - -unsigned int COpendriveRoadSegment::get_num_signals(void) const -{ - return this->signals.size(); -} - -const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index) const -{ - std::stringstream error; - - if(index>=0 && index<this->signals.size()) - return *this->signals[index]; - else - { - error << "Invalid signal index (" << index << ") for segment " << this->name; - throw CException(_HERE_,error.str()); - } -} - -unsigned int COpendriveRoadSegment::get_num_objects(void) const -{ - return this->objects.size(); -} - -const COpendriveObject &COpendriveRoadSegment::get_object(unsigned int index) const -{ - std::stringstream error; - - if(index>=0 && index<this->objects.size()) - return *this->objects[index]; - else - { - error << "Invalid object index (" << index << ") for segment " << this->name; - throw CException(_HERE_,error.str()); - } -} - -unsigned int COpendriveRoadSegment::get_num_connecting(void) const -{ - return this->connecting.size(); -} - -const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int index) const -{ - std::stringstream error; - - if(index>=0 && index<this->connecting.size()) - return *this->connecting[index]; - else - { - error << "Invalid connecting segment index (" << index << ") for segment " << this->name; - throw CException(_HERE_,error.str()); - } -} - -double COpendriveRoadSegment::get_length(void) const -{ - double length=0.0; - - for(unsigned int i=0;i<this->geometries.size();i++) - length+=this->geometries[i].spline->get_length(); - - return length; -} - -TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length) const -{ - TOpendriveWorldPose world_pose{0}; - TPoint spline_pose; - - for(unsigned int i=0;i<this->geometries.size();i++) - { - if((length-this->geometries[i].spline->get_length())<0.0) - { - spline_pose=this->geometries[i].spline->evaluate(length); - world_pose.x=spline_pose.x; - world_pose.y=spline_pose.y; - world_pose.heading=spline_pose.heading; - return world_pose; - } - else - length-=this->geometries[i].spline->get_length(); - } - - return world_pose; -} - -double COpendriveRoadSegment::get_curvature_at(double length) const -{ - double curvature; - TPoint spline_pose; - - for(unsigned int i=0;i<this->geometries.size();i++) - { - if((length-this->geometries[i].spline->get_length())<0.0) - { - spline_pose=this->geometries[i].spline->evaluate(length); - curvature=spline_pose.curvature; - return curvature; - } - else - length-=this->geometries[i].spline->get_length(); - } - - return 0.0; -} - -TOpendriveLocalPose COpendriveRoadSegment::transform_to_local_pose(const TOpendriveTrackPose &track) -{ - TOpendriveTrackPose pose; - TOpendriveLocalPose local; - std::stringstream error; - - if(track.s<0.0) - { - error << "Invalid track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")"; - throw CException(_HERE_,error.str()); - } - pose=track; - for(unsigned int i=0;i<this->geometries.size();i++) - { - if((pose.s-this->geometries[i].opendrive->get_length())<=this->resolution) - { - if(!this->geometries[i].opendrive->get_local_pose(pose,local)) - { - error << "Impossible to transform to local coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in segment " << this->name; - throw CException(_HERE_,error.str()); - } - return local; - } - else - pose.s-=this->geometries[i].opendrive->get_length(); - } - - error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to segment " << this->name; - throw CException(_HERE_,error.str()); -} - -TOpendriveWorldPose COpendriveRoadSegment::transform_to_world_pose(const TOpendriveTrackPose &track) -{ - TOpendriveTrackPose pose; - TOpendriveWorldPose world; - std::stringstream error; - - if(track.s<0.0) - { - error << "Invalid track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")"; - throw CException(_HERE_,error.str()); - } - pose=track; - for(unsigned int i=0;i<this->geometries.size();i++) - { - if((pose.s-this->geometries[i].opendrive->get_length())<=this->resolution) - { - if(!this->geometries[i].opendrive->get_world_pose(pose,world)) - { - error << "Impossible to transform to world coordinates pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in segment " << this->name; - throw CException(_HERE_,error.str()); - } - return world; - } - else - pose.s-=this->geometries[i].opendrive->get_length(); - } - - error << "Track pose (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to segment " << this->name; - throw CException(_HERE_,error.str()); -} - -unsigned int COpendriveRoadSegment::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const -{ - double dist,min_dist=std::numeric_limits<double>::max(),length; - TOpendriveWorldPose closest_pose; - unsigned int closest_index=-1; - - for(unsigned int i=0;i<this->nodes.size();i++) - { - length=this->nodes[i]->get_closest_pose(pose,closest_pose,false,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; - } - } - } - - return closest_index; -} - -const COpendriveRoadNode &COpendriveRoadSegment::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]; -} - -int COpendriveRoadSegment::get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol) const -{ - double dist,min_dist=std::numeric_limits<double>::max(),length; - TOpendriveWorldPose closest_pose; - int closest_id=0; - std::map<int,COpendriveLane *>::const_iterator lane_it; - - for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++) - { - length=lane_it->second->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_id=lane_it->first; - distance=length; - } - } - } - - return closest_id; -} - -const COpendriveLane &COpendriveRoadSegment::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol) const -{ - std::map<int,COpendriveLane *>::const_iterator lane_it; - - int closest_id=this->get_closest_lane_id(pose,distance,angle_tol); - if(closest_id==0) - throw CException(_HERE_,"Impossible to find the closest lane"); - for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++) - if(lane_it->first==closest_id) - return *lane_it->second; - - throw CException(_HERE_,"Impossible to find the closest lane"); -} - -double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const -{ - double dist,min_dist=std::numeric_limits<double>::max(),distance,length; - unsigned int closest_index=-1; - TPoint closest_spline_point; - - closest_pose.x=std::numeric_limits<double>::max(); - closest_pose.y=std::numeric_limits<double>::max(); - closest_pose.heading=std::numeric_limits<double>::max(); - for(unsigned int i=0;i<this->geometries.size();i++) - { - length=this->geometries[i].spline->find_closest_point(pose.x,pose.y,closest_spline_point); - if(length<std::numeric_limits<double>::max()) - { - dist=sqrt(pow(closest_spline_point.x-pose.x,2.0)+pow(closest_spline_point.y-pose.y,2.0)); - if(dist<min_dist) - { - min_dist=dist; - closest_index=i; - closest_pose.x=closest_spline_point.x; - closest_pose.y=closest_spline_point.y; - closest_pose.heading=normalize_angle(closest_spline_point.heading); - distance=length; - } - } - } - - if(closest_index!=(unsigned int)-1) - { - for(unsigned int i=0;i<closest_index;i++) - distance+=this->geometries[i].spline->get_length(); - } - else - distance=std::numeric_limits<double>::max(); - - return distance; -} - -std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment) -{ - out << "Road " << segment.get_name() << " (" << segment.get_id() << ")" << std::endl; - out << " Connecting road segments: " << segment.connecting.size() << std::endl; - for(unsigned int i=0;i<segment.connecting.size();i++) - out << " " << segment.connecting[i]->get_name() << std::endl; - out << " Number of nodes: " << segment.get_num_nodes() << std::endl; - for(unsigned int i=0;i<segment.get_num_nodes();i++) - out << " " << segment.get_node(i).get_index() << std::endl; - out << " Number of right lanes: " << segment.num_right_lanes << std::endl; - for(int i=-1;i>=-segment.num_right_lanes;i--) - out << *segment.lanes[i]; - out << " Number of left lanes: " << segment.num_left_lanes << std::endl; - for(int i=1;i<=segment.num_left_lanes;i++) - out << *segment.lanes[i]; - out << " Number of signals: " << segment.signals.size() << std::endl; - for(unsigned int i=0;i<segment.signals.size();i++) - out << *segment.signals[i]; - out << " Number of objects: " << segment.objects.size() << std::endl; - for(unsigned int i=0;i<segment.objects.size();i++) - out << *segment.objects[i]; - - return out; -} - -COpendriveRoadSegment::~COpendriveRoadSegment() -{ - this->free(); - this->name=""; - this->id=-1; - this->center_mark=OD_MARK_NONE; - this->resolution=DEFAULT_RESOLUTION; - this->scale_factor=DEFAULT_SCALE_FACTOR; - this->min_road_length=DEFAULT_MIN_ROAD_LENGTH; -} - diff --git a/src/osm/osm_junction.cpp b/src/osm/osm_junction.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9e5ec0f299d54916f32557aa6a5b7a7eaf4f0ff5 --- /dev/null +++ b/src/osm/osm_junction.cpp @@ -0,0 +1,807 @@ +#include "osm/osm_junction.h" + +#include <math.h> + +#include <iostream> + +COSMJunction::COSMJunction(COSMNode *node) +{ + std::vector<COSMWay *>::iterator it; + + this->parent_node=node; + node->junction=this; + + for(it=node->ways.begin();it!=node->ways.end();it++) + { + if((*it)->is_node_first(node->id)) + { + this->out_roads.push_back((*it)->road_segment); + if(!(*it)->is_one_way()) + this->in_roads.push_back((*it)->road_segment); + (*it)->road_segment->start_junction=this; + } + else if((*it)->is_node_last(node->id)) + { + this->in_roads.push_back((*it)->road_segment); + if(!(*it)->is_one_way()) + this->out_roads.push_back((*it)->road_segment); + (*it)->road_segment->end_junction=this; + } + } + this->create_initial_connectivity(); + this->apply_node_restrictions(); + this->apply_way_restrictions(); + this->create_links(); +} + +void COSMJunction::create_FF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix) +{ + COSMNode *check_node; + + check_node=&out_way.get_next_node_by_id(this->parent_node->id); + if(in_way.is_node_left(*check_node,true,0.2)) + { + if((num_in_lanes-in_lane_index-1)==(num_out_lanes-out_lane_index-1)) + matrix(in_lane_index,out_lane_index)=1; + else if(((num_out_lanes-out_lane_index-1)>=num_in_lanes) && in_lane_index==0) + matrix(in_lane_index,out_lane_index)=1; + } + else if(in_way.is_node_right(*check_node,true,0.2)) + { + if(in_lane_index==out_lane_index) + matrix(in_lane_index,out_lane_index)=1; + else if((out_lane_index>=num_in_lanes) && in_lane_index==(num_in_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + } + else + { + if(in_lane_index==out_lane_index) + matrix(in_lane_index,out_lane_index)=1; + else if(in_lane_index>=num_out_lanes && out_lane_index==(num_out_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + else if((out_lane_index>=num_in_lanes) && in_lane_index==(num_in_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + } +} + +void COSMJunction::create_FB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix) +{ + COSMNode *check_node; + + check_node=&out_way.get_prev_node_by_id(this->parent_node->id); + if(in_way.is_node_left(*check_node,true,0.2)) + { + if((num_in_lanes-in_lane_index-1)==(out_lane_index-(total_out_lanes-num_out_lanes))) + matrix(in_lane_index,out_lane_index)=1; + else if(((out_lane_index-(total_out_lanes-num_out_lanes))>=num_in_lanes) && in_lane_index==0) + matrix(in_lane_index,out_lane_index)=1; + } + else if(in_way.is_node_right(*check_node,true,0.2)) + { + if(in_lane_index==(total_out_lanes-out_lane_index-1)) + matrix(in_lane_index,out_lane_index)=1; + else if(((total_out_lanes-out_lane_index-1)>=num_in_lanes) && in_lane_index==(num_in_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + } + else + { + if(in_lane_index==(total_out_lanes-out_lane_index-1)) + matrix(in_lane_index,out_lane_index)=1; + else if(in_lane_index>=num_out_lanes && (total_out_lanes-out_lane_index-1)==(num_out_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + else if(((total_out_lanes-out_lane_index-1)>=num_in_lanes) && in_lane_index==(num_in_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + } +} + +void COSMJunction::create_BF_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix) +{ + COSMNode *check_node; + + check_node=&out_way.get_next_node_by_id(this->parent_node->id); + if(in_way.is_node_left(*check_node,false,0.2)) + { + if((in_lane_index-(total_in_lanes-num_in_lanes))==(num_out_lanes-out_lane_index-1)) + matrix(in_lane_index,out_lane_index)=1; + else if(((signed int)out_lane_index<((signed int)num_out_lanes-(signed int)num_in_lanes)) && (total_in_lanes-in_lane_index-1)==0) + matrix(in_lane_index,out_lane_index)=1; + } + else if(in_way.is_node_right(*check_node,false,0.2)) + { + if((total_in_lanes-in_lane_index-1)==out_lane_index) + matrix(in_lane_index,out_lane_index)=1; + else if((out_lane_index>=num_in_lanes) && (total_in_lanes-in_lane_index-1)==(num_in_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + } + else + { + if((total_in_lanes-in_lane_index-1)==out_lane_index) + matrix(in_lane_index,out_lane_index)=1; + else if((total_in_lanes-in_lane_index-1)>=num_out_lanes && out_lane_index==(num_out_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + else if((out_lane_index>=num_in_lanes) && (total_in_lanes-in_lane_index-1)==(num_in_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + } +} + +void COSMJunction::create_BB_connectivity_matrix(COSMWay &in_way,COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int total_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix) +{ + COSMNode *check_node; + + check_node=&out_way.get_prev_node_by_id(this->parent_node->id); + if(in_way.is_node_left(*check_node,false,0.2)) + { + if((in_lane_index-(total_in_lanes-num_in_lanes))==out_lane_index) + matrix(in_lane_index,out_lane_index)=1; + else if((out_lane_index>=num_in_lanes) && (total_in_lanes-in_lane_index-1)==0) + matrix(in_lane_index,out_lane_index)=1; + } + else if(in_way.is_node_right(*check_node,false,0.2)) + { + if((total_in_lanes-in_lane_index-1)==(total_out_lanes-out_lane_index-1)) + matrix(in_lane_index,out_lane_index)=1; + else if(((total_out_lanes-out_lane_index-1)>=num_in_lanes) && (total_in_lanes-in_lane_index-1)==(num_in_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + } + else + { + if((total_in_lanes-in_lane_index-1)==(total_out_lanes-out_lane_index-1)) + matrix(in_lane_index,out_lane_index)=1; + else if((total_in_lanes-in_lane_index-1)>=num_out_lanes && (total_out_lanes-out_lane_index-1)==(num_out_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + else if(((total_out_lanes-out_lane_index-1)>=num_in_lanes) && (total_in_lanes-in_lane_index-1)==(num_in_lanes-1)) + matrix(in_lane_index,out_lane_index)=1; + } +} + +void COSMJunction::create_initial_connectivity(void) +{ + this->connections.resize(this->in_roads.size()); + for(unsigned int i=0;i<this->in_roads.size();i++) + { + COSMWay &in_way=this->in_roads[i]->get_parent_way(); + this->connections[i].resize(this->out_roads.size()); + for(unsigned int j=0;j<this->out_roads.size();j++) + { + COSMWay &out_way=this->out_roads[j]->get_parent_way(); + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + for(unsigned int k=0;k<in_way.get_num_lanes();k++) + { + for(unsigned int l=0;l<out_way.get_num_lanes();l++) + { + if(in_way.get_id()!=out_way.get_id()) + { + /* + if(out_way.is_node_first(this->parent_node->id)) + { + if(k>=in_way.get_num_forward_lanes() && l<out_way.get_num_forward_lanes()) + { + if((k-in_way.get_num_forward_lanes())==(out_way.get_num_forward_lanes()-l-1)) + this->connections[i][j](k,l)=1; + else if(((out_way.get_num_forward_lanes()-l-1)>in_way.get_num_backward_lanes()) && k==(in_way.get_num_lanes()-1)) + this->connections[i][j](k,l)=1; + } + } + else + { + if(k<in_way.get_num_forward_lanes() && l>=out_way.get_num_forward_lanes()) + { + if((in_way.get_num_forward_lanes()-k-1)==(l-out_way.get_num_forward_lanes())) + this->connections[i][j](k,l)=1; + else if(((l-out_way.get_num_forward_lanes())>=in_way.get_num_forward_lanes()) && k==0) + this->connections[i][j](k,l)=1; + } + } + } + else + { + */ + if(in_way.is_one_way()) + { + if(out_way.is_one_way()) + this->create_FF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_lanes(),out_way.get_num_lanes(),this->connections[i][j]); + else + { + if(out_way.is_node_first(this->parent_node->id)) + { + if(l<out_way.get_num_forward_lanes()) + this->create_FF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_lanes(),out_way.get_num_forward_lanes(),this->connections[i][j]); + } + else + { + if(l>=out_way.get_num_forward_lanes()) + this->create_FB_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_lanes(),out_way.get_num_backward_lanes(),out_way.get_num_lanes(),this->connections[i][j]); + } + } + } + else + { + if(out_way.is_one_way()) + { + if(in_way.is_node_last(this->parent_node->id)) + { + if(k<in_way.get_num_forward_lanes()) + this->create_FF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_forward_lanes(),out_way.get_num_lanes(),this->connections[i][j]); + } + else + { + if(k>=in_way.get_num_forward_lanes()) + this->create_BF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_backward_lanes(),in_way.get_num_lanes(),out_way.get_num_lanes(),this->connections[i][j]); + } + } + else + { + if(in_way.is_node_last(this->parent_node->id)) + { + if(out_way.is_node_first(this->parent_node->id)) + { + if(k<in_way.get_num_forward_lanes() && l<out_way.get_num_forward_lanes()) + this->create_FF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_forward_lanes(),out_way.get_num_forward_lanes(),this->connections[i][j]); + } + else + { + if(k<in_way.get_num_forward_lanes() && l>=out_way.get_num_forward_lanes()) + this->create_FB_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_forward_lanes(),out_way.get_num_backward_lanes(),out_way.get_num_lanes(),this->connections[i][j]); + } + } + else + { + if(out_way.is_node_first(this->parent_node->id)) + { + if(k>=in_way.get_num_forward_lanes() && l<out_way.get_num_forward_lanes()) + this->create_BF_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_backward_lanes(),in_way.get_num_lanes(),out_way.get_num_forward_lanes(),this->connections[i][j]); + } + else + { + if(k>=in_way.get_num_forward_lanes() && l>=out_way.get_num_forward_lanes()) + this->create_BB_connectivity_matrix(in_way,out_way,k,l,in_way.get_num_backward_lanes(),in_way.get_num_lanes(),out_way.get_num_backward_lanes(),out_way.get_num_lanes(),this->connections[i][j]); + } + } + } + } + } + } + } + } + } + +} + +void COSMJunction::apply_node_restrictions(void) +{ + COSMNode *check_node; + + for(unsigned int k=0;k<this->parent_node->get_num_restrictions();k++) + { + COSMRestriction &res=this->parent_node->get_restriction(k); + for(unsigned int i=0;i<this->in_roads.size();i++) + { + COSMWay &in_way=this->in_roads[i]->get_parent_way(); + for(unsigned int j=0;j<this->out_roads.size();j++) + { + COSMWay &out_way=this->out_roads[j]->get_parent_way(); + switch(res.get_action()) + { + case RESTRICTION_NO: + if(out_way.is_node_first(this->parent_node->id)) + check_node=&out_way.get_next_node_by_id(this->parent_node->id); + else + check_node=&out_way.get_prev_node_by_id(this->parent_node->id); + switch(res.get_type()) + { + case RESTRICTION_LEFT_TURN: + if(res.get_from_way().get_id()==in_way.get_id() && res.get_to_way().get_id()==out_way.get_id()) + { + if(in_way.is_node_first(this->parent_node->id)) + { + if(k>=in_way.get_num_forward_lanes()) + { + if(in_way.is_node_left(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(in_way.is_node_left(*check_node,false)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + else + { + if(k>=in_way.get_num_forward_lanes()) + { + if(in_way.is_node_left(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(in_way.is_node_left(*check_node,false)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + } + break; + case RESTRICTION_RIGHT_TURN: + if(res.get_from_way().get_id()==in_way.get_id() && res.get_to_way().get_id()==out_way.get_id()) + { + if(in_way.is_node_first(this->parent_node->id)) + { + if(k>=in_way.get_num_forward_lanes()) + { + if(in_way.is_node_right(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(in_way.is_node_right(*check_node,false)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + else + { + if(k>=in_way.get_num_forward_lanes()) + { + if(in_way.is_node_right(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(in_way.is_node_right(*check_node,false)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + } + break; + case RESTRICTION_U_TURN: + if(res.get_from_way().get_id()==in_way.get_id() && res.get_to_way().get_id()==out_way.get_id()) + { + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + break; + default: + break; + } + break; + case RESTRICTION_ONLY: + if(out_way.is_node_first(this->parent_node->id)) + check_node=&out_way.get_next_node_by_id(this->parent_node->id); + else + check_node=&out_way.get_prev_node_by_id(this->parent_node->id); + switch(res.get_type()) + { + case RESTRICTION_LEFT_TURN: + if(res.get_from_way().get_id()==in_way.get_id()) + { + if(in_way.is_one_way()) + { + if(out_way.is_one_way()) + { + if(!in_way.is_node_left(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(!in_way.is_node_left(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + else + { + if(in_way.is_node_first(this->parent_node->id)) + { + if(k>=in_way.get_num_forward_lanes()) + { + if(!in_way.is_node_left(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(!in_way.is_node_left(*check_node,false)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + else + { + if(k>=in_way.get_num_forward_lanes()) + { + if(!in_way.is_node_left(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(!in_way.is_node_left(*check_node,false)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + } + } + break; + case RESTRICTION_RIGHT_TURN: + if(res.get_from_way().get_id()==in_way.get_id()) + { + if(in_way.is_one_way()) + { + if(out_way.is_one_way()) + { + if(!in_way.is_node_right(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(!in_way.is_node_right(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + else + { + if(in_way.is_node_first(this->parent_node->id)) + { + if(k>=in_way.get_num_forward_lanes()) + { + if(!in_way.is_node_right(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(!in_way.is_node_right(*check_node,false)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + else + { + if(k>=in_way.get_num_forward_lanes()) + { + if(!in_way.is_node_right(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(!in_way.is_node_right(*check_node,false)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + } + } + break; + case RESTRICTION_STRAIGHT_ON: + if(res.get_from_way().get_id()==in_way.get_id()) + { + if(in_way.is_one_way()) + { + if(out_way.is_one_way()) + { + if(in_way.is_node_right(*check_node,true) || in_way.is_node_left(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(in_way.is_node_right(*check_node,true) || in_way.is_node_left(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + else + { + if(in_way.is_node_first(this->parent_node->id)) + { + if(in_way.is_node_right(*check_node,false) || in_way.is_node_left(*check_node,false)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + else + { + if(in_way.is_node_right(*check_node,true) || in_way.is_node_left(*check_node,true)) + this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes()); + } + } + } + break; + default: + break; + } + break; + default: + break; + } + } + } + } +} + +void COSMJunction::apply_way_restrictions(void) +{ + lane_restructions_t restriction; + COSMNode *check_node; + bool left,right; + + for(unsigned int i=0;i<this->in_roads.size();i++) + { + COSMWay &in_way=this->in_roads[i]->get_parent_way(); + for(unsigned int j=0;j<this->out_roads.size();j++) + { + COSMWay &out_way=this->out_roads[j]->get_parent_way(); + if(out_way.is_node_first(this->parent_node->id)) + check_node=&out_way.get_next_node_by_id(this->parent_node->id); + else + check_node=&out_way.get_prev_node_by_id(this->parent_node->id); + for(unsigned int k=0;k<in_way.get_num_lanes();k++) + { + if(in_way.is_node_first(this->parent_node->id)) + { + if(k<in_way.get_num_forward_lanes()) + { + left=in_way.is_node_left(*check_node,false,0.2); + right=in_way.is_node_right(*check_node,false,0.2); + } + else + { + left=in_way.is_node_left(*check_node,true,0.2); + right=in_way.is_node_right(*check_node,true,0.2); + } + } + else + { + if(k<in_way.get_num_forward_lanes()) + { + left=in_way.is_node_left(*check_node,true,0.2); + right=in_way.is_node_right(*check_node,true,0.2); + } + else + { + left=in_way.is_node_left(*check_node,false,0.2); + right=in_way.is_node_right(*check_node,false,0.2); + } + } + restriction=in_way.get_lane_restriction(k); + if(restriction==NO_RESTRICTION) + continue; + if(!(restriction&RESTRICTION_THROUGH))//can't go through + { + if(!left && !right) + { + for(unsigned int l=0;l<out_way.get_num_lanes();l++) + this->connections[i][j](k,l)=0; + } + } + if(!(restriction&RESTRICTION_LEFT))//can't go through + { + if(left) + { + for(unsigned int l=0;l<out_way.get_num_lanes();l++) + this->connections[i][j](k,l)=0; + } + } + if(!(restriction&RESTRICTION_RIGHT))//can't go through + { + if(right) + { + for(unsigned int l=0;l<out_way.get_num_lanes();l++) + this->connections[i][j](k,l)=0; + } + } + } + } + } +} + +void COSMJunction::create_links(void) +{ + TOSMLink new_link; + COSMNode *node1,*node3; + double length1,length2,angle,in_radius,out_radius,in_dist,out_dist; + bool left=false,right=false; + + for(unsigned int i=0;i<this->connections.size();i++) + { + COSMWay &in_way=this->in_roads[i]->get_parent_way(); + for(unsigned int j=0;j<this->connections[i].size();j++) + { + COSMWay &out_way=this->out_roads[j]->get_parent_way(); + for(unsigned int k=0;k<in_way.get_num_lanes();k++) + { + for(unsigned int l=0;l<out_way.get_num_lanes();l++) + { + if(this->connections[i][j](k,l)==1)// link exists + { + new_link.in_way=&in_way; + new_link.out_way=&out_way; + new_link.in_lane_id=k; + new_link.out_lane_id=l; + if(out_way.is_node_first(this->parent_node->get_id())) + node3=&out_way.get_segment_end_node(FIRST_SEGMENT); + else + node3=&out_way.get_segment_start_node(LAST_SEGMENT); + in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0)*in_way.get_lane_width(); + if(in_way.is_node_first(this->parent_node->get_id())) + { + node1=&in_way.get_segment_end_node(FIRST_SEGMENT); + left=in_way.is_node_left(*node3,false,0.2); + right=in_way.is_node_right(*node3,false,0.2); + } + else + { + node1=&in_way.get_segment_start_node(LAST_SEGMENT); + left=in_way.is_node_left(*node3,true,0.2); + right=in_way.is_node_right(*node3,true,0.2); + } + /* + if(in_way.is_node_first(this->parent_node->get_id())) + { + node1=&in_way.get_segment_end_node(FIRST_SEGMENT); + if((left=in_way.is_node_left(*node3,false,0.2))==true) + in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-k)*in_way.get_lane_width(); + else if((right=in_way.is_node_right(*node3,false,0.2))==true) + in_radius=DEFAULT_MIN_RADIUS+(k+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width(); + } + else + { + node1=&in_way.get_segment_start_node(LAST_SEGMENT); + if((left=in_way.is_node_left(*node3,true,0.2))==true) + in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0)*in_way.get_lane_width(); + else if((right=in_way.is_node_right(*node3,true,0.2))==true) + in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-k)*in_way.get_lane_width(); + } + */ + out_radius=DEFAULT_MIN_RADIUS+(out_way.get_num_lanes()/2.0)*out_way.get_lane_width(); + /* + if(out_way.is_node_first(this->parent_node->get_id())) + { + if(left) + out_radius=DEFAULT_MIN_RADIUS+(l+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width(); + else if(right) + out_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-l)*in_way.get_lane_width(); + } + else + { + if(left) + out_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-l)*in_way.get_lane_width(); + else if(right) + out_radius=DEFAULT_MIN_RADIUS+(l+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width(); + } + */ +// std::cout << "in way: " << in_way.id << " lane " << k << " out way: " << out_way.id << " lane " << l << " in radius " << in_radius << " out radius " << out_radius << " num lanes in: " << in_way.get_num_lanes() << " num fwd lanes in: " << in_way.get_num_forward_lanes() << " num lanes out: " << out_way.get_num_lanes() << " num fwd lanes out: " << out_way.get_num_forward_lanes() << std::endl; + angle=this->parent_node->compute_angle(*node1,*node3); + if(!left && !right) + { + length1=this->parent_node->compute_distance(*node1); + length2=this->parent_node->compute_distance(*node3); + in_dist=std::min(length1,length2)/2.0; + if(in_dist>DEFAULT_MIN_RADIUS) + in_dist=DEFAULT_MIN_RADIUS; + else if(in_dist<MIN_ROAD_LENGTH/2.0) + in_dist=MIN_ROAD_LENGTH/2.0; + out_dist=in_dist; + } + else + { + in_dist=(out_radius+in_radius*cos(angle))/fabs(sin(angle)); + if(in_dist<MIN_ROAD_LENGTH/2.0) + in_dist=MIN_ROAD_LENGTH/2.0; + out_dist=(in_radius+out_radius*cos(angle))/fabs(sin(angle)); + if(out_dist<MIN_ROAD_LENGTH/2.0) + out_dist=MIN_ROAD_LENGTH; + } + if(this->add_link(new_link)) + { + if(in_way.is_node_first(this->parent_node->get_id())) + in_way.road_segment->set_start_distance(in_dist); + else + in_way.road_segment->set_end_distance(in_dist); + if(out_way.is_node_first(this->parent_node->get_id())) + out_way.road_segment->set_start_distance(out_dist); + else + out_way.road_segment->set_end_distance(out_dist); + } + } + } + } + } + } +} + +bool COSMJunction::add_link(TOSMLink &new_link) +{ + for(unsigned int i=0;i<this->links.size();i++) + { + if(new_link.in_way->get_id()==this->links[i].in_way->get_id() && new_link.in_lane_id==this->links[i].in_lane_id && new_link.out_way->get_id()==this->links[i].out_way->get_id() && new_link.out_lane_id==this->links[i].out_lane_id) + return false; + } + this->links.push_back(new_link); + + return true; +} + +void COSMJunction::convert(CJunction **junction,osm_road_map_t &road_map) +{ + CRoad *in=NULL,*out=NULL; + unsigned int from_index,to_index; + + (*junction)=new CJunction(); + + // add incomming and outgoing roads + for(unsigned int i=0;i<this->links.size();i++) + { + for(osm_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++) + { + if(it->first->parent_way->get_id()==this->links[i].in_way->get_id()) + { + if(it->first->parent_way->is_node_first(this->parent_node->get_id())) + { + if((it->second).size()>1) + { + in=(it->second)[1]; + from_index=this->links[i].in_lane_id-it->first->parent_way->get_num_forward_lanes(); + if(!(*junction)->has_incomming_road(in->get_id())) + (*junction)->add_incomming_road(in); + } + } + else if(it->first->parent_way->is_node_last(this->parent_node->get_id())) + { + if((it->second).size()>0) + { + in=(it->second)[0]; + from_index=it->first->parent_way->get_num_forward_lanes()-this->links[i].in_lane_id-1; + if(!(*junction)->has_incomming_road(in->get_id())) + (*junction)->add_incomming_road(in); + } + } + else + continue; + } + else if(it->first->parent_way->get_id()==this->links[i].out_way->get_id()) + { + if(it->first->parent_way->is_node_first(this->parent_node->get_id())) + { + if((it->second).size()>0) + { + out=(it->second)[0]; + to_index=it->first->parent_way->get_num_forward_lanes()-this->links[i].out_lane_id-1; + if(!(*junction)->has_outgoing_road(out->get_id())) + (*junction)->add_outgoing_road(out); + } + } + else if(it->first->parent_way->is_node_last(this->parent_node->get_id())) + { + if((it->second).size()>1) + { + out=(it->second)[1]; + to_index=this->links[i].out_lane_id-it->first->parent_way->get_num_forward_lanes(); + if(!(*junction)->has_outgoing_road(out->get_id())) + (*junction)->add_outgoing_road(out); + } + } + } + else + continue; + } + if(in!=NULL && out!=NULL) + { + if(!(*junction)->are_roads_linked_by_id(in->get_id(),out->get_id())) + (*junction)->link_roads_by_id(in->get_id(),out->get_id()); + (*junction)->link_lanes_by_id(in->get_id(),from_index,out->get_id(),to_index); + } + } +} + +COSMNode &COSMJunction::get_parent_node(void) +{ + return *this->parent_node; +} + +std::ostream& operator<<(std::ostream& out, COSMJunction &junction) +{ + out << " ID: " << junction.parent_node->get_id() << std::endl; + out << " number of in roads: " << junction.in_roads.size() << std::endl; + for(unsigned int i=0;i<junction.in_roads.size();i++) + out << " Road ID: " << junction.in_roads[i]->get_parent_way().get_id() << std::endl; + out << " number of out roads: " << junction.out_roads.size() << std::endl; + for(unsigned int i=0;i<junction.out_roads.size();i++) + out << " Road ID: " << junction.out_roads[i]->get_parent_way().get_id() << std::endl; + + return out; +} + +COSMJunction::~COSMJunction() +{ + this->parent_node=NULL; + this->in_roads.clear(); + this->out_roads.clear(); + this->links.clear(); +} diff --git a/src/osm/osm_map.cpp b/src/osm/osm_map.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c9d5967e86a8f832da9010e995a427fca13a52b6 --- /dev/null +++ b/src/osm/osm_map.cpp @@ -0,0 +1,382 @@ +#include "osm/osm_map.h" +#include "exceptions.h" + +#include <iostream> + +#include <osmium/io/xml_input.hpp> +#include <osmium/visitor.hpp> +#include <osmium/tags/taglist.hpp> +#include <GeographicLib/UTMUPS.hpp> + +#include "road.h" + +const std::vector<std::string> highway_tags={"motorway","trunk","primary","secondary","tertiary","motor_way_link","trunk_link","primary_link","secondary_link","tertiary_link","minor","residential","living","service","living_street"}; + +const std::vector<std::string> relation_type_tags={"restriction"}; + +COSMMap::COSMMap() : + way_filter{false} +{ + for(unsigned int i=0;i<highway_tags.size();i++) + this->way_filter.add_rule(true, "highway", highway_tags[i]); + for(unsigned int i=0;i<relation_type_tags.size();i++) + this->relation_filter.add_rule(true, "type", relation_type_tags[i]); + this->max_id=-std::numeric_limits<long int>::max(); + this->utm_zone=31; +} + +void COSMMap::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->junctions.size();i++) + { + delete this->junctions[i]; + this->junctions[i]=NULL; + } + this->junctions.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->ways.size();i++) + { + delete this->ways[i]; + this->ways[i]=NULL; + } + this->ways.clear(); + for(unsigned int i=0;i<this->restrictions.size();i++) + { + delete this->restrictions[i]; + this->restrictions[i]=NULL; + } + this->ways.clear(); +} + +void COSMMap::process_map(void) +{ + std::vector<COSMNode *>::iterator node_it; + std::vector<COSMWay *>::iterator way_it; + + // remove all nodes that do not belong to any road + for(node_it=this->nodes.begin();node_it!=this->nodes.end();) + { + if((*node_it)->get_num_ways()==0) + node_it=this->nodes.erase(node_it); + else + node_it++; + } + // merge and split ways as necessary + for(unsigned int i=0;i<this->nodes.size();i++) + { + if(!this->nodes[i]->merge_ways()) + this->nodes[i]->split_ways(); + } + // remove unconnected ways + for(way_it=this->ways.begin();way_it!=this->ways.end();) + { + if((*way_it)->is_not_connected()) + way_it=this->ways.erase(way_it); + else + way_it++; + } + // remove colinear nodes in ways + for(unsigned int i=0;i<this->ways.size();i++) + this->ways[i]->remove_straight_nodes(); + // remove nodes inside other ways width + for(unsigned int i=0;i<this->nodes.size();i++) + this->nodes[i]->remove_in_junction_nodes(); +} + +void COSMMap::create_junctions(void) +{ + COSMJunction *new_junction; + + for(unsigned int i=0;i<this->nodes.size();i++) + { + if(this->nodes[i]->get_num_ways()>1) + { + new_junction=new COSMJunction(this->nodes[i]); + this->junctions.push_back(new_junction); + } + } +} + +void COSMMap::create_road_segments(void) +{ + COSMRoadSegment *new_segment; + + for(unsigned int i=0;i<this->ways.size();i++) + { + new_segment=new COSMRoadSegment(this->ways[i]); + this->segments.push_back(new_segment); + } +} + +void COSMMap::delete_way(COSMWay *way) +{ + std::vector<COSMWay *>::iterator it; + + for(it=this->ways.begin();it!=this->ways.end();++it) + if(*it==way) + { + this->ways.erase(it); + delete way; + break; + } +} + +void COSMMap::add_way(COSMWay *way) +{ + if(way->id==-1) + { + this->max_id++; + way->id=this->max_id; + } + else + { + if(way->id>this->max_id) + this->max_id=way->id; + for(unsigned int i=0;i<this->ways.size();i++) + if(this->ways[i]->id==way->id) + return; + } + this->ways.push_back(way); +} + +COSMWay *COSMMap::get_way_by_id(long int id) +{ + for(unsigned int i=0;i<this->ways.size();i++) + if(this->ways[i]->get_id()==id) + return this->ways[i]; + + return NULL; +} + +void COSMMap::delete_node(COSMNode *node) +{ + std::vector<COSMNode *>::iterator it; + + for(it=this->nodes.begin();it!=this->nodes.end();++it) + if(*it==node) + { + this->nodes.erase(it); + delete node; + break; + } +} + +void COSMMap::add_node(COSMNode *node) +{ + if(node->id==-1) + { + this->max_id++; + node->id=this->max_id; + } + else + { + if(node->id>this->max_id) + this->max_id=node->id; + for(unsigned int i=0;i<this->nodes.size();i++) + if(this->nodes[i]->id==node->id) + return; + } + this->nodes.push_back(node); +} + +COSMNode *COSMMap::get_node_by_id(long int id) +{ + for(unsigned int i=0;i<this->nodes.size();i++) + if(this->nodes[i]->get_id()==id) + return this->nodes[i]; + + return NULL; +} + +void COSMMap::add_restriction(COSMRestriction *restriction) +{ + if(restriction->id==-1) + { + this->max_id++; + restriction->id=this->max_id; + } + else + { + if(restriction->id>this->max_id) + this->max_id=restriction->id; + for(unsigned int i=0;i<this->restrictions.size();i++) + if(this->restrictions[i]->get_id()==restriction->id) + return; + } + this->restrictions.push_back(restriction); +} + +void COSMMap::normalize_locations(std::vector<osmium::Box> &boxes) +{ + double max_x,min_x,max_y,min_y,center_x,center_y,gamma,k; + int zone; + bool northp; + + if(boxes.size()!=1) + { + this->free(); + throw CException(_HERE_,"Invalid number of bounding boxes. Only one supported"); + } + osmium::Location top_right=boxes[0].top_right(); + osmium::Location bottom_left=boxes[0].bottom_left(); + GeographicLib::UTMUPS::Forward(top_right.lat(),top_right.lon(),zone,northp,max_x,max_y,gamma,k,this->utm_zone); + GeographicLib::UTMUPS::Forward(bottom_left.lat(),bottom_left.lon(),zone,northp,min_x,min_y,gamma,k,this->utm_zone); + center_x=(max_x+min_x)/2.0; + center_y=(max_y+min_y)/2.0; + for(unsigned int i=0;i<this->nodes.size();i++) + this->nodes[i]->normalize_location(center_x,center_y); +} + +void COSMMap::set_utm_zone(int zone) +{ + this->utm_zone=zone; +} + +int COSMMap::get_utm_zone(void) +{ + return this->utm_zone; +} + +void COSMMap::load(const std::string &filename) +{ + osmium::osm_entity_bits::type read_types; + + this->free(); + // read the nodes + read_types = osmium::osm_entity_bits::node; + osmium::io::Reader node_reader{filename, read_types}; + osmium::apply(node_reader,*this); + node_reader.close(); + + // read the ways + read_types = osmium::osm_entity_bits::way; + osmium::io::Reader way_reader{filename, read_types}; + osmium::apply(way_reader,*this); + way_reader.close(); + + // read relations + read_types = osmium::osm_entity_bits::relation; + osmium::io::Reader relation_reader{filename, read_types}; + osmium::apply(relation_reader,*this); + relation_reader.close(); + + // read the bounds of the map + std::vector<osmium::Box> boxes=node_reader.header().boxes(); + this->normalize_locations(boxes); + + this->process_map(); + this->create_road_segments(); + this->create_junctions(); +} + +void COSMMap::node(const osmium::Node& node) +{ + COSMNode *new_node; + + // create and add the new node + new_node = new COSMNode(node,this); + this->add_node(new_node); +} + +void COSMMap::way(const osmium::Way& way) +{ + COSMWay *new_way=NULL; + + if(osmium::tags::match_any_of(way.tags(),this->way_filter)) + { + try{ + new_way=new COSMWay(way,this); + this->add_way(new_way); + }catch(CException &e){ + if(new_way!=NULL) + delete new_way; + } + } +} + +void COSMMap::relation(const osmium::Relation& relation) +{ + COSMRestriction *new_restriction=NULL; + + if(osmium::tags::match_any_of(relation.tags(),this->relation_filter)) + { + try{ + new_restriction=new COSMRestriction(relation,this); + this->add_restriction(new_restriction); + }catch(CException &e){ + if(new_restriction!=NULL) + delete new_restriction; + } + } +} + +void COSMMap::convert(CRoadMap &road) +{ + CRoad *left_road,*right_road; + CJunction *junction; + osm_road_map_t road_map; + osm_map_pair_t map_pair; + std::vector<CRoad *> segment_roads; + + for(unsigned int i=0;i<this->segments.size();i++) + { + this->segments[i]->convert(&left_road,&right_road,road.get_resolution()); + segment_roads.clear(); + if(right_road!=NULL) + { + road.add_road(right_road); + segment_roads.push_back(right_road); + } + if(left_road!=NULL) + { + road.add_road(left_road); + segment_roads.push_back(left_road); + } + if(right_road!=NULL || left_road!=NULL) + { + map_pair=std::make_pair(this->segments[i],segment_roads); + road_map.push_back(map_pair); + } + } + for(unsigned int i=0;i<this->junctions.size();i++) + { + this->junctions[i]->convert(&junction,road_map); + if(junction!=NULL) + road.add_junction(junction); + } +} + +std::ostream& operator<<(std::ostream& out, COSMMap &map) +{ + out << "Number of nodes: " << map.nodes.size() << std::endl; + for(unsigned int i=0;i<map.nodes.size();i++) + out << *map.nodes[i] << std::endl; + out << "Number of junctions: " << map.junctions.size() << std::endl; + for(unsigned int i=0;i<map.junctions.size();i++) + out << *map.junctions[i] << std::endl; + out << "Number of ways: " << map.ways.size() << std::endl; + for(unsigned int i=0;i<map.ways.size();i++) + out << *map.ways[i] << std::endl; + out << "Number of roads: " << map.segments.size() << std::endl; + for(unsigned int i=0;i<map.segments.size();i++) + out << *map.segments[i] << std::endl; + + return out; +} + +COSMMap::~COSMMap() +{ + this->free(); +} + diff --git a/src/osm/osm_node.cpp b/src/osm/osm_node.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fdc1808d74b58fcf4de37057c1066c12fc8a5e31 --- /dev/null +++ b/src/osm/osm_node.cpp @@ -0,0 +1,366 @@ +#include "osm/osm_node.h" +#include "exceptions.h" + +#include <iostream> +#include <sstream> + +#include <math.h> + +#include <osmium/geom/coordinates.hpp> +#include <osmium/geom/mercator_projection.hpp> +#include <GeographicLib/UTMUPS.hpp> + +COSMNode::COSMNode(const osmium::Node &node,COSMMap *parent) +{ + int zone; + bool northp; + double gamma,k; + + this->id=node.id(); + GeographicLib::UTMUPS::Forward(node.location().lat(),node.location().lon(),zone,northp,this->location.x,this->location.y,gamma,k,parent->get_utm_zone()); + this->ways.clear(); + this->junction=NULL; + this->restrictions.clear(); + this->parent=parent; +} + +COSMNode::COSMNode(const COSMNode &object) +{ + this->id=object.id; + this->ways=object.ways; + this->location.x=object.location.x; + this->location.y=object.location.y; + this->junction=object.junction; + this->restrictions=object.restrictions; + this->parent=object.parent; +} + +void COSMNode::set_parent(COSMMap *parent) +{ + this->parent=parent; +} + +void COSMNode::add_way(COSMWay *new_way) +{ + for(unsigned int i=0;i<this->ways.size();i++) + if(this->ways[i]->id==new_way->id) + return; + this->ways.push_back(new_way); +} + +void COSMNode::delete_way(COSMWay *way) +{ + std::vector<COSMWay *>::iterator it; + + for(it=this->ways.begin();it!=this->ways.end();) + { + if((*it)==way) + it=this->ways.erase(it); + else + it++; + } +} + +void COSMNode::update_way(COSMWay *old_way,COSMWay *new_way) +{ + std::vector<COSMWay *>::iterator it; + + for(it=this->ways.begin();it!=this->ways.end();++it) + { + if((*it)==old_way) + (*it)=new_way; + } +} + +bool COSMNode::merge_ways(void) +{ + COSMWay *way1,*way2; + + if(this->ways.size()==2) + { + way1=*this->ways.begin(); + way2=*this->ways.rbegin(); + + if((way1->is_node_first(this->id) || way1->is_node_last(this->id)) && + (way2->is_node_first(this->id) || way2->is_node_last(this->id))) + { + if(way1->get_num_forward_lanes()==way2->get_num_forward_lanes() && + way1->get_num_backward_lanes()==way2->get_num_backward_lanes()) + { + // merge both ways into one + if(way1->is_node_last(this->id) && way2->is_node_first(this->id)) + { + way1->merge(way2); + this->parent->delete_way(way2); + return true; + } + else if(way1->is_node_first(this->id) && way2->is_node_last(this->id)) + { + way2->merge(way1); + this->parent->delete_way(way1); + return true; + } + else + return false; + } + else + return false; + } + else + return false; + } + else + return false; +} + +void COSMNode::split_ways(void) +{ + std::vector<COSMWay *>::iterator it; + std::vector<COSMWay *> new_ways; + COSMWay *new_way; + + new_ways.clear(); + if(this->ways.size()>1) + { + for(it=this->ways.begin();it!=this->ways.end();++it) + { + if(!(*it)->is_node_first(id) && !(*it)->is_node_last(id)) + { + new_way=(*it)->split(this->id); + if(new_way!=NULL) + new_ways.push_back(new_way); + } + } + for(unsigned int i=0;i<new_ways.size();i++) + this->add_way(new_ways[i]); + } + else + { + if(this->ways[0]->get_node_multiplicity(this->id)>1) + { + + } + } +} + +void COSMNode::remove_in_junction_nodes(void) +{ + double width,width2,dist,x,y,heading,length,start_x,start_y,end_x,end_y,cross,angle; + COSMNode *check_node,*node1,*node2; + + if(this->ways.size()>1) + { + for(unsigned int i=0;i<this->ways.size();i++) + { + width=this->ways[i]->get_num_lanes()*this->ways[i]->get_lane_width()/2.0; + if(this->ways[i]->is_node_first(this->id)) + { + node1=(COSMNode *)&this->ways[i]->get_segment_end_node(FIRST_SEGMENT); + node2=(COSMNode *)&this->ways[i]->get_segment_start_node(FIRST_SEGMENT); + } + else + { + node2=(COSMNode *)&this->ways[i]->get_segment_end_node(LAST_SEGMENT); + node1=(COSMNode *)&this->ways[i]->get_segment_start_node(LAST_SEGMENT); + } + node1->get_location(start_x,start_y); + node2->get_location(end_x,end_y); + length=node1->compute_distance(*node2); + for(unsigned int j=0;j<this->ways.size();j++) + { + if(j!=i) + { + if(this->ways[j]->is_node_first(this->id)) + check_node=(COSMNode *)&this->ways[j]->get_segment_end_node(FIRST_SEGMENT); + else + check_node=(COSMNode *)&this->ways[j]->get_segment_start_node(LAST_SEGMENT); + angle=this->compute_angle(*node1,*check_node); + if(fabs(angle)<1.5707) + { + width2=this->ways[j]->get_num_lanes()*this->ways[j]->get_lane_width()/2.0; + heading=this->compute_heading(*check_node); + check_node->get_location(x,y); + x=x-width2*sin(heading); + y=y+width2*cos(heading); + cross=(x-start_x)*(end_y-start_y)-(y-start_y)*(end_x-start_x); + dist=fabs(cross)/length; + if(dist<width) + { + this->ways[j]->delete_node(check_node); + continue; + } + check_node->get_location(x,y); + x=x+width2*sin(heading); + y=y-width2*cos(heading); + cross=(x-start_x)*(end_y-start_y)-(y-start_y)*(end_x-start_x); + dist=fabs(cross)/length; + if(dist<width) + this->ways[j]->delete_node(check_node); + } + } + } + } + } +} + +void COSMNode::add_restriction(COSMRestriction *new_restriction) +{ + for(unsigned int i=0;i<this->restrictions.size();i++) + if(this->restrictions[i]->id==new_restriction->id) + return; + this->restrictions.push_back(new_restriction); +} + +void COSMNode::delete_restriction(COSMRestriction *restriction) +{ + std::vector<COSMRestriction *>::iterator it; + + for(it=this->restrictions.begin();it!=this->restrictions.end();it++) + { + if((*it)==restriction) + { + this->restrictions.erase(it); + break; + } + } +} + +void COSMNode::clear_restrictions(void) +{ + this->restrictions.clear(); +} + +void COSMNode::normalize_location(double center_x,double center_y) +{ + this->location.x-=center_x; + this->location.y-=center_y; +} + +long int COSMNode::get_id(void) +{ + return this->id; +} + +unsigned int COSMNode::get_num_ways(void) +{ + return this->ways.size(); +} + +COSMWay &COSMNode::get_way_by_index(unsigned int index) +{ + unsigned int i; + std::stringstream text; + std::vector<COSMWay *>::iterator it; + + for(it=this->ways.begin(),i=0;it!=this->ways.end();++it,i++) + if(i==index) + return **it; + text << "Node with id " << this->id << " does not belong to so many ways"; + throw CException(_HERE_,text.str()); +} + +COSMWay &COSMNode::get_way_by_id(long int id) +{ + std::stringstream text; + + for(unsigned int i=0;i<this->ways.size();i++) + if(this->ways[i]->id==id) + return *this->ways[i]; + text << "Node with id " << this-> id << " does not belong to way with id " << id; + throw CException(_HERE_,text.str()); +} + +void COSMNode::get_location(double &x, double &y) +{ + x=this->location.x; + y=this->location.y; +} + +double COSMNode::compute_distance(COSMNode &node) +{ + double distance; + + distance=sqrt(pow(this->location.x-node.location.x,2.0)+pow(this->location.y-node.location.y,2.0)); + + return distance; +} + +double COSMNode::compute_heading(COSMNode &node) +{ + double heading; + + heading=atan2(node.location.y-this->location.y,node.location.x-this->location.x); + if(heading<0.0) + heading+=2.0*3.14159; + + return heading; +} + +double COSMNode::compute_angle(COSMNode &node1,COSMNode &node2) +{ + double angle,cross; + double dist01,dist02,dist12; + double start_x,start_y,end_x,end_y; + + dist01=this->compute_distance(node1); + node1.get_location(start_x,start_y); + dist02=this->compute_distance(node2); + node2.get_location(end_x,end_y); + dist12=node1.compute_distance(node2); + + cross=(this->location.x-start_x)*(end_y-start_y)-(this->location.y-start_y)*(end_x-start_x); + angle=acos((pow(dist01,2.0)+pow(dist02,2.0)-pow(dist12,2.0))/(2.0*dist01*dist02)); + if(cross>=0.0) + angle=-angle; + + return angle; +} + +unsigned int COSMNode::get_num_restrictions(void) +{ + return this->restrictions.size(); +} + +COSMRestriction &COSMNode::get_restriction(unsigned int index) +{ + unsigned int i; + std::stringstream text; + std::vector<COSMRestriction *>::iterator it; + + for(it=this->restrictions.begin(),i=0;it!=this->restrictions.end();++it,i++) + if(i==index) + return **it; + text << "Node with id " << this->id << " does not have so many restrictions"; + throw CException(_HERE_,text.str()); +} + +COSMMap &COSMNode::get_parent(void) +{ + return *this->parent; +} + +std::ostream& operator<<(std::ostream& out,COSMNode &node) +{ + double x,y; + + out << " id: " << node.get_id() << std::endl; + node.get_location(x,y); + out << " location: x: " << x << " y: " << y << std::endl; + out << " number of ways: " << node.get_num_ways() << std::endl; + for(unsigned int i=0;i<node.get_num_ways();i++) + out << " way id: " << node.get_way_by_index(i).get_id() << std::endl; + + return out; +} + +COSMNode::~COSMNode() +{ + this->location.x=0.0; + this->location.y=0.0; + this->id=-1; + this->ways.clear(); + this->junction=NULL; + this->restrictions.clear(); + this->parent=NULL; +} + diff --git a/src/osm/osm_restriction.cpp b/src/osm/osm_restriction.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ded5c665388e499d551c534038e600fb0b71e9d8 --- /dev/null +++ b/src/osm/osm_restriction.cpp @@ -0,0 +1,178 @@ +#include "osm/osm_restriction.h" +#include "exceptions.h" + +#include <iostream> + +COSMRestriction::COSMRestriction(COSMWay *from,COSMNode *via,COSMWay *to,restriction_action_t action, restriction_type_t type,COSMMap *parent) +{ + this->parent=parent; + this->id=-1; + this->from=from; + this->via=via; + this->to=to; + this->action=action; + this->type=type; +} + +COSMRestriction::COSMRestriction(const osmium::Relation &relation,COSMMap *parent) +{ + COSMNode *node; + COSMWay *way; + const char *value; + osmium::RelationMemberList::const_iterator it; + const osmium::RelationMemberList &members=relation.members(); + bool from_missing=true,to_missing=true,via_missing=true; + + for(it=members.begin();it!=members.end();++it) + { + std::string role=std::string(it->role()); + if(role.compare("from")==0) + { + way=parent->get_way_by_id(it->ref()); + if(way!=NULL) + { + this->from=way; + from_missing=false; + } + else + throw CException(_HERE_,"From way does not exist"); + } + else if(role.compare("to")==0) + { + way=parent->get_way_by_id(it->ref()); + if(way!=NULL) + { + this->to=way; + to_missing=false; + } + else + throw CException(_HERE_,"To way does not exist"); + } + else if(role.compare("via")==0) + { + node=parent->get_node_by_id(it->ref()); + if(node!=NULL) + { + this->via=node; + via_missing=false; + } + else + throw CException(_HERE_,"Via node does not exist"); + } + else + throw CException(_HERE_,"Invalid restriction role"); + } + if(via_missing || from_missing || to_missing) + throw CException(_HERE_,"Incomplete restriction from->via->to information"); + value=relation.get_value_by_key("restriction"); + if(value!=NULL) + { + std::string restriction_text(value); + std::string action=restriction_text.substr(0,restriction_text.find_first_of("_")); + if(action.compare("no")==0) + this->action=RESTRICTION_NO; + else if(action.compare("only")==0) + this->action=RESTRICTION_ONLY; + else + throw CException(_HERE_,"Unsupported restriction action"); + std::string restriction=restriction_text.substr(restriction_text.find_first_of("_")+1); + if(restriction.compare("left_turn")==0) + this->type=RESTRICTION_LEFT_TURN; + else if(restriction.compare("right_turn")==0) + this->type=RESTRICTION_RIGHT_TURN; + else if(restriction.compare("u_turn")==0) + this->type=RESTRICTION_U_TURN; + else if(restriction.compare("straight_on")==0) + this->type=RESTRICTION_STRAIGHT_ON; + else + throw CException(_HERE_,"Unsupported restriction type"); + } + else + throw CException(_HERE_,"Missing restriction data"); + this->parent=parent; + this->id=relation.id(); + this->from->add_restriction(this); + this->via->add_restriction(this); + this->to->add_restriction(this); +} + +COSMRestriction::COSMRestriction(const COSMRestriction &object) +{ + this->id=object.id; + this->from=object.from; + this->to=object.to; + this->via=object.via; + this->action=object.action; + this->type=object.type; + this->parent=object.parent; +} + +COSMRestriction::COSMRestriction() +{ + this->id=-1; + this->from=NULL; + this->to=NULL; + this->via=NULL; + this->action=RESTRICTION_NONE; + this->parent=NULL; +} + +void COSMRestriction::set_parent(COSMMap *parent) +{ + this->parent=parent; +} + +void COSMRestriction::update_to_way(COSMWay *old_way,COSMWay *new_way) +{ + if(this->to->get_id()==old_way->get_id()) + this->to=new_way; +} + +void COSMRestriction::update_from_way(COSMWay *old_way,COSMWay *new_way) +{ + if(this->from->get_id()==old_way->get_id()) + this->from=new_way; +} + +long int COSMRestriction::get_id(void) +{ + return this->id; +} + +COSMWay &COSMRestriction::get_from_way(void) +{ + return *this->from; +} + +COSMWay &COSMRestriction::get_to_way(void) +{ + return *this->to; +} + +COSMNode &COSMRestriction::get_via_node(void) +{ + return *this->via; +} + +restriction_action_t COSMRestriction::get_action(void) +{ + return this->action; +} + +restriction_type_t COSMRestriction::get_type(void) +{ + return this->type; +} + +COSMMap &COSMRestriction::get_parent(void) +{ + return *this->parent; +} + +COSMRestriction::~COSMRestriction() +{ + this->from=NULL; + this->to=NULL; + this->via=NULL; + this->parent=NULL; +} diff --git a/src/osm/osm_road_segment.cpp b/src/osm/osm_road_segment.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7ed8da2e771be666316ba716f513c68d18d02870 --- /dev/null +++ b/src/osm/osm_road_segment.cpp @@ -0,0 +1,419 @@ +#include "osm/osm_road_segment.h" +#include "exceptions.h" + +COSMRoadSegment::COSMRoadSegment(COSMWay *way) +{ + this->parent_way=way; + way->road_segment=this; + this->start_junction=NULL; + this->end_junction=NULL; + this->start_distance=0.0; + this->original_start_distance=0.0; + this->end_distance=0.0; + this->original_end_distance=0.0; +} + +void COSMRoadSegment::set_start_distance(double dist) +{ + COSMNode *node1,*node2; + double length,start_end_distance; + + if(dist>this->original_start_distance) + this->original_start_distance=dist; + if(dist>this->start_distance) + { + node1=&this->parent_way->get_segment_start_node(FIRST_SEGMENT); + node2=&this->parent_way->get_segment_end_node(FIRST_SEGMENT); + length=node1->compute_distance(*node2); + if(this->parent_way->get_num_nodes()==2) + { + if((length-this->end_distance-dist)<MIN_ROAD_LENGTH) + { + if(this->original_end_distance==0.0) + this->start_distance=length-MIN_ROAD_LENGTH; + else + { + start_end_distance=this->original_end_distance+dist; + this->end_distance=this->original_end_distance*length/start_end_distance; + this->end_distance-=MIN_ROAD_LENGTH/2.0; + this->start_distance=dist*length/start_end_distance; + this->start_distance-=MIN_ROAD_LENGTH/2.0; + } + } + else + this->start_distance=dist; + } + else + { + if((length-dist)<MIN_ROAD_LENGTH) + this->start_distance=length-MIN_ROAD_LENGTH; + else + this->start_distance=dist; + } + } +} + +void COSMRoadSegment::set_end_distance(double dist) +{ + COSMNode *node1,*node2; + double length,start_end_distance; + + if(dist>this->original_end_distance) + this->original_end_distance=dist; + if(dist>this->end_distance) + { + node1=&this->parent_way->get_segment_start_node(LAST_SEGMENT); + node2=&this->parent_way->get_segment_end_node(LAST_SEGMENT); + length=node1->compute_distance(*node2); + if(this->parent_way->get_num_nodes()==2) + { + if((length-this->start_distance-dist)<MIN_ROAD_LENGTH) + { + if(this->original_start_distance==0.0) + this->end_distance=length-MIN_ROAD_LENGTH; + else + { + start_end_distance=dist+this->original_start_distance; + this->end_distance=dist*length/start_end_distance; + this->end_distance-=MIN_ROAD_LENGTH/2.0; + this->start_distance=this->original_start_distance*length/start_end_distance; + this->start_distance-=MIN_ROAD_LENGTH/2.0; + } + } + else + this->end_distance=dist; + } + else + { + if((length-dist)<MIN_ROAD_LENGTH) + this->end_distance=length-MIN_ROAD_LENGTH; + else + this->end_distance=dist; + } + } +} + +bool COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution) +{ + double heading1,heading2,x,y,angle,length1,dist,prev_dist,y_offset; +// double angle2,angle3,prev_heading; + TPoint start_point,end_point; + COSMNode *node1,*node2,*node3; + + y_offset=((double)this->parent_way->get_num_forward_lanes()-((double)this->parent_way->get_num_lanes()/2.0))*this->parent_way->get_lane_width(); + if(this->parent_way->get_num_nodes()<2) + throw CException(_HERE_,"Road segment does not have enough nodes to generate a geometry"); + prev_dist=this->start_distance; + for(unsigned int i=0;i<=this->parent_way->get_num_nodes()-2;i++) + { + node1=&this->parent_way->get_node_by_index(i); + node2=&this->parent_way->get_node_by_index(i+1); + length1=node1->compute_distance(*node2); + heading1=node1->compute_heading(*node2); + node1->get_location(x,y); + if(i==0) + { + start_point.x=x+prev_dist*cos(heading1)-y_offset*sin(heading1); + start_point.y=y+prev_dist*sin(heading1)+y_offset*cos(heading1); + start_point.heading=heading1; + start_point.curvature=0.0; + road->set_start_point(start_point); +// prev_heading=heading1; + } + if((i+1)==(this->parent_way->get_num_nodes()-1))// straight line + { +// angle2=atan2(y_offset,length1-this->end_distance); +// angle3=diff_angle(heading1,prev_heading)+angle2; + if((length1-this->end_distance-prev_dist)>resolution)// && fabs(angle3)<1.5707) + { + end_point.x=x+(length1-this->end_distance)*cos(heading1)-y_offset*sin(heading1); + end_point.y=y+(length1-this->end_distance)*sin(heading1)+y_offset*cos(heading1); + end_point.heading=heading1; + end_point.curvature=0.0; + road->add_segment(end_point); + } + } + else + { + node3=&this->parent_way->get_node_by_index(i+2); + heading2=node2->compute_heading(*node3); + angle=node2->compute_angle(*node1,*node3); + dist=(DEFAULT_MIN_RADIUS+y_offset)/fabs(tan(angle/2.0)); + if(dist>(length1-prev_dist)) + { + if(i==0) + { + node2->get_location(x,y); + start_point.x=x-y_offset*sin(heading2); + start_point.y=y+y_offset*cos(heading2); + start_point.heading=heading2; + start_point.curvature=0.0; + road->set_start_point(start_point); + prev_dist=0.0; +// prev_heading=heading2; + } + else if((i+2)==(this->parent_way->get_num_nodes()-1)) + { + prev_dist=dist; +// prev_heading=heading2; + } + else + { + prev_dist=dist; +// prev_heading=heading1; + } + continue; + } + else if(dist<MIN_ROAD_LENGTH/2.0) + dist=MIN_ROAD_LENGTH/2.0; + if(length1-prev_dist-dist>resolution)// straight segment + { + end_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1); + end_point.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1); + end_point.heading=heading1; + end_point.curvature=0.0; + road->add_segment(end_point); + } + // arc between the two segments + node2->get_location(x,y); + end_point.x=x+dist*cos(heading2)-y_offset*sin(heading2); + end_point.y=y+dist*sin(heading2)+y_offset*cos(heading2); + end_point.heading=heading2; + end_point.curvature=0.0; + road->add_segment(end_point); + prev_dist=dist; +// prev_heading=heading1; + } + } + if(road->get_num_segments()==0) + return false; + else + return true; +} + +bool COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution) +{ + double heading1,heading2,x,y,angle,length1,dist,prev_dist,y_offset; +// double angle2,angle3,prev_heading; + TPoint start_point,end_point; + COSMNode *node1,*node2,*node3; + + y_offset=-((double)this->parent_way->get_num_forward_lanes()-((double)this->parent_way->get_num_lanes()/2.0))*this->parent_way->get_lane_width(); + if(this->parent_way->get_num_nodes()<2) + throw CException(_HERE_,"Road segment does not have enough nodes to generate a geometry"); + prev_dist=this->end_distance; + for(unsigned int i=this->parent_way->get_num_nodes()-1;i>=1;i--) + { + node1=&this->parent_way->get_node_by_index(i); + node2=&this->parent_way->get_node_by_index(i-1); + length1=node1->compute_distance(*node2); + heading1=node1->compute_heading(*node2); + node1->get_location(x,y); + if(i==(this->parent_way->get_num_nodes()-1)) + { + start_point.x=x+prev_dist*cos(heading1)-y_offset*sin(heading1); + start_point.y=y+prev_dist*sin(heading1)+y_offset*cos(heading1); + start_point.heading=heading1; + start_point.curvature=0.0; + road->set_start_point(start_point); +// prev_heading=heading1; + } + if((i-1)==0)// straight line + { +// angle2=atan2(y_offset,length1-this->end_distance); +// angle3=diff_angle(heading1,prev_heading)+angle2; +// if(fabs(angle3)<1.5707) + if((length1-this->start_distance-prev_dist)>resolution)// && fabs(angle3)<1.5707) + { + end_point.x=x+(length1-this->start_distance)*cos(heading1)-y_offset*sin(heading1); + end_point.y=y+(length1-this->start_distance)*sin(heading1)+y_offset*cos(heading1); + end_point.heading=heading1; + end_point.curvature=0.0; + road->add_segment(end_point); + } + } + else + { + node3=&this->parent_way->get_node_by_index(i-2); + heading2=node2->compute_heading(*node3); + angle=node2->compute_angle(*node1,*node3); + dist=(DEFAULT_MIN_RADIUS+y_offset)/fabs(tan(angle/2.0)); + if(dist>(length1-prev_dist)) + { + if(i==(this->parent_way->get_num_nodes()-1)) + { + node2->get_location(x,y); + start_point.x=x-y_offset*sin(heading2); + start_point.y=y+y_offset*cos(heading2); + start_point.heading=heading2; + start_point.curvature=0.0; + road->set_start_point(start_point); + prev_dist=0.0; +// prev_heading=heading2; + } + else if((i-2)==0) + { + prev_dist=dist; +// prev_heading=heading2; + } + else + { + prev_dist=dist; +// prev_heading=heading1; + } + continue; + } + else if(dist<MIN_ROAD_LENGTH/2.0) + dist=MIN_ROAD_LENGTH/2.0; + if(length1-prev_dist-dist>resolution)// straight segment + { + end_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1); + end_point.y=y+(length1-dist)*sin(heading1)+y_offset*cos(heading1); + end_point.heading=heading1; + end_point.curvature=0.0; + road->add_segment(end_point); + } + // arc between the two segments + node2->get_location(x,y); + end_point.x=x+dist*cos(heading2)-y_offset*sin(heading2); + end_point.y=y+dist*sin(heading2)+y_offset*cos(heading2); + end_point.heading=heading2; + end_point.curvature=0.0; + road->add_segment(end_point); + prev_dist=dist; +// prev_heading=heading1; + } + } + if(road->get_num_segments()==0) + return false; + else + return true; +} + +void COSMRoadSegment::convert(CRoad **left_road,CRoad **right_road,double resolution) +{ + double lane_width; + lane_restructions_t restrictions; + CRoadSegment *segment; + unsigned int num_lanes; + + num_lanes=this->get_parent_way().get_num_forward_lanes(); + lane_width=this->get_parent_way().get_lane_width(); + if(num_lanes>0) + { + (*right_road)=new CRoad(); + (*right_road)->set_num_lanes(num_lanes); + (*right_road)->set_lane_width(lane_width); + // generate geometry + if(this->generate_fwd_geometry(*right_road,resolution)) + { + // generate connectivity + for(unsigned int i=0;i<(*right_road)->get_num_segments();i++) + { + segment=(*right_road)->get_segment_by_index(i); + for(unsigned int j=0;j<num_lanes;j++) + { + restrictions=this->get_parent_way().get_lane_restriction(this->get_parent_way().get_num_forward_lanes()-j-1); + if(restrictions&RESTRICTION_RIGHT) + { + for(unsigned int k=j+1;k<num_lanes;k++) + segment->unlink_lanes(j,k); + } + if(restrictions&RESTRICTION_LEFT) + { + for(unsigned int k=0;k<j;k++) + segment->unlink_lanes(j,k); + } +// if(restrictions&RESTRICTION_THROUGH) +// segment->unlink_lanes(j,j); + } + } + } + else + { + delete (*right_road); + (*right_road)=NULL; + } + } + else + (*right_road)=NULL; + num_lanes=this->get_parent_way().get_num_backward_lanes(); + if(num_lanes>0) + { + (*left_road)=new CRoad(); + (*left_road)->set_num_lanes(num_lanes); + (*left_road)->set_lane_width(lane_width); + // generate geometry + if(this->generate_bwd_geometry(*left_road,resolution)) + { + // generate connectivity + for(unsigned int i=0;i<(*left_road)->get_num_segments();i++) + { + segment=(*left_road)->get_segment_by_index(i); + for(unsigned int j=0;j<num_lanes;j++) + { + restrictions=this->get_parent_way().get_lane_restriction(j+this->get_parent_way().get_num_forward_lanes()); + if(restrictions&RESTRICTION_RIGHT) + { + for(unsigned int k=j+1;k<num_lanes;k++) + segment->unlink_lanes(j,k); + } + if(restrictions&RESTRICTION_LEFT) + { + for(unsigned int k=0;k<j;k++) + segment->unlink_lanes(j,k); + } +// if(restrictions&RESTRICTION_THROUGH) +// segment->unlink_lanes(j,j); + } + } + } + else + { + delete (*left_road); + (*left_road)=NULL; + } + } + else + (*left_road)=NULL; + if((*right_road)!=NULL && (*left_road)!=NULL) + { + (*left_road)->set_opposite_direction_road((*right_road)); + (*right_road)->set_opposite_direction_road((*left_road)); + } +} + +COSMWay &COSMRoadSegment::get_parent_way(void) +{ + return *this->parent_way; +} + +double COSMRoadSegment::get_start_distance(void) +{ + return this->start_distance; +} + +double COSMRoadSegment::get_end_distance(void) +{ + return this->end_distance; +} + +std::ostream& operator<<(std::ostream& out, COSMRoadSegment &segment) +{ + out << " ID: " << segment.parent_way->get_id() << std::endl; + if(segment.start_junction!=NULL) + out << " start junction ID: " << segment.start_junction->get_parent_node().get_id() << std::endl; + else + out << " NO start junction" << std::endl; + if(segment.end_junction!=NULL) + out << " end junction ID: " << segment.end_junction->get_parent_node().get_id() << std::endl; + else + out << " No end junction" << std::endl; + + return out; +} + +COSMRoadSegment::~COSMRoadSegment() +{ +} + diff --git a/src/osm/osm_way.cpp b/src/osm/osm_way.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e07adfe28af8245eccd017b0eb4b6fe958dc25d8 --- /dev/null +++ b/src/osm/osm_way.cpp @@ -0,0 +1,803 @@ +#include "osm/osm_way.h" +#include "exceptions.h" + +#include <iostream> +#include <sstream> + +COSMWay::COSMWay(const osmium::Way &way,COSMMap *parent) +{ + static unsigned int road_id=0; + std::stringstream new_name; + osmium::NodeRefList::const_iterator it; + COSMNode *node; + const char *value; + unsigned int i=0; + + this->id=way.id(); + value=way.get_value_by_key("name"); + if(value!=NULL) + this->name=value; + else + { + new_name << "road" << road_id; + this->name=new_name.str(); + road_id++; + } + // get the number of lanes + value=way.get_value_by_key("lanes:forward"); + if(value==NULL) + { + value=way.get_value_by_key("lanes"); + if(value==NULL) + { + value=way.get_value_by_key("oneway"); + if(value!=NULL) + { + if(std::string(value).compare("no")==0) + { + this->num_forward_lanes=1; + this->num_backward_lanes=1; + } + else + { + this->num_forward_lanes=1; + this->num_backward_lanes=0; + } + } + else + { + this->num_forward_lanes=1; + this->num_backward_lanes=0; + } + } + else + { + this->num_forward_lanes=std::stoi(std::string(value)); + if(this->num_forward_lanes>1) + { + value=way.get_value_by_key("oneway"); + if(value!=NULL) + { + if(std::string(value).compare("no")==0) + { + this->num_forward_lanes--; + this->num_backward_lanes=1; + } + else + this->num_backward_lanes=0; + } + else + this->num_backward_lanes=0; + } + else + this->num_backward_lanes=0; + + } + } + else + { + this->num_forward_lanes=std::stoi(std::string(value)); + value=way.get_value_by_key("lanes:backward"); + if(value==NULL) + this->num_backward_lanes=0; + else + this->num_backward_lanes=std::stoi(std::string(value)); + } + // get the road width +// value=way.get_value_by_key("width"); +// if(value==NULL) +// this->lane_width=-1.0; +// else +// this->lane_width=std::stof(std::string(value))/(this->num_forward_lanes+this->num_backward_lanes); + this->lane_width=DEFAULT_LANE_WIDTH; + // get the lane constraints + this->load_lane_restrictions(way); + // create and add the way + const osmium::WayNodeList &nodes=way.nodes(); + for(it=nodes.begin(),i=0;it!=nodes.end();++it,i++) + { + node=parent->get_node_by_id(it->ref()); + if(node!=NULL) + { + this->nodes.push_back(node); + node->add_way(this); + } + } + this->road_segment=NULL; + this->parent=parent; + this->restrictions.clear(); +} + +COSMWay::COSMWay(const COSMWay &object) +{ + this->id=object.id; + this->name=object.name; + this->nodes=object.nodes; + this->num_forward_lanes=object.num_forward_lanes; + this->forward_lanes=object.forward_lanes; + this->num_backward_lanes=object.num_backward_lanes; + this->backward_lanes=object.backward_lanes; + this->lane_width=object.lane_width; + this->road_segment=object.road_segment; + this->parent=object.parent; + this->restrictions=object.restrictions; +} + +bool COSMWay::load_turn_lane_tag(const osmium::Way &way,const std::string &tag,std::vector<lane_restructions_t> &restrictions,unsigned int max_lanes) +{ + const char *value; + std::size_t prev_pos=0,pos; + std::vector<std::string> way_restrictions; + std::vector<std::string> lane_restrictions; + + value=way.get_value_by_key(tag.c_str()); + if(value!=NULL) + { + restrictions.clear(); + std::string restriction_text(value); + while((pos=restriction_text.find_first_of("|",prev_pos))!=std::string::npos) + { + way_restrictions.push_back(restriction_text.substr(prev_pos,pos-prev_pos)); + prev_pos=pos+1; + } + way_restrictions.push_back(restriction_text.substr(prev_pos)); + if(max_lanes<way_restrictions.size()) + restrictions.resize(max_lanes,(lane_restructions_t)0); + else + restrictions.resize(way_restrictions.size(),(lane_restructions_t)0); + for(unsigned int i=0;i<way_restrictions.size() && i<max_lanes;i++) + { + lane_restrictions.clear(); + prev_pos=0; + while((pos=way_restrictions[i].find_first_of(";",prev_pos))!=std::string::npos) + { + lane_restrictions.push_back(way_restrictions[i].substr(prev_pos,pos-prev_pos)); + prev_pos=pos+1; + } + lane_restrictions.push_back(way_restrictions[i].substr(prev_pos)); + for(unsigned int j=0;j<lane_restrictions.size();j++) + { + if(lane_restrictions[j].compare("through")==0) + restrictions[max_lanes-i-1]=(lane_restructions_t)(restrictions[max_lanes-i-1]|RESTRICTION_THROUGH); + else if(lane_restrictions[j].compare("left")==0) + restrictions[max_lanes-i-1]=(lane_restructions_t)(restrictions[max_lanes-i-1]|RESTRICTION_LEFT); + else if(lane_restrictions[j].compare("right")==0) + restrictions[max_lanes-i-1]=(lane_restructions_t)(restrictions[max_lanes-i-1]|RESTRICTION_RIGHT); + else if(lane_restrictions[j].compare("")==0) + restrictions[max_lanes-i-1]=NO_RESTRICTION; + else + throw CException(_HERE_,"Unknown lane restriction"); + } + } + return true; + } + else + return false; +} + +void COSMWay::load_lane_restrictions(const osmium::Way &way) +{ + std::vector<lane_restructions_t> restrictions; + + this->forward_lanes.resize(this->num_forward_lanes,(lane_restructions_t)0); + this->backward_lanes.resize(this->num_backward_lanes,(lane_restructions_t)0); + + if(this->load_turn_lane_tag(way,"turn:lanes",restrictions,this->num_forward_lanes)) + this->forward_lanes=restrictions; + else + { + if(this->load_turn_lane_tag(way,"turn:lanes:forward",restrictions,this->num_forward_lanes)) + this->forward_lanes=restrictions; + if(this->load_turn_lane_tag(way,"turn:lanes:backward",restrictions,this->num_backward_lanes)) + this->backward_lanes=restrictions; + } +} + +void COSMWay::set_parent(COSMMap *parent) +{ + this->parent=parent; +} + +void COSMWay::add_node(COSMNode *new_node) +{ + for(unsigned int i=0;i<this->nodes.size();i++) + if(this->nodes[i]->id==new_node->id) + return; + this->nodes.push_back(new_node); + new_node->add_way(this); +} + +void COSMWay::delete_node(COSMNode *node) +{ + std::vector<COSMNode *>::iterator it; + + if(!this->is_node_first(node->get_id()) && !this->is_node_last(node->get_id())) + { + for(it=this->nodes.begin();it!=nodes.end();it++) + { + if((*it)==node) + { + this->nodes.erase(it); + this->parent->delete_node(node); + break; + } + } + } +} + +void COSMWay::update_node(COSMNode *old_node,COSMNode *new_node) +{ + std::vector<COSMNode *>::iterator it; + + for(it=this->nodes.begin();it!=this->nodes.end();++it) + { + if((*it)==old_node) + (*it)=new_node; + } +} + +bool COSMWay::is_node_first(long int id) +{ + if(this->nodes[0]->id==id) + return true; + else + return false; +} + +bool COSMWay::is_node_last(long int id) +{ + if(this->nodes[this->nodes.size()-1]->id==id) + return true; + else + return false; +} + +bool COSMWay::is_node_left(COSMNode &node,bool forward,double tol) +{ + COSMNode *start_node,*end_node; + unsigned int closest_seg; + double angle; + + closest_seg=this->get_closest_segment(node); + if(forward) + { + start_node=&this->get_segment_start_node(closest_seg); + end_node=&this->get_segment_end_node(closest_seg); + angle=end_node->compute_angle(*start_node,node); + if(angle<0.0 && fabs(angle)<(3.14159-tol) && fabs(angle)>tol) + return true; + else + return false; + } + else + { + start_node=&this->get_segment_start_node(closest_seg); + end_node=&this->get_segment_end_node(closest_seg); + angle=start_node->compute_angle(*end_node,node); + if(angle<0.0 && fabs(angle)<(3.14159-tol) && fabs(angle)>tol) + return true; + else + return false; + } +} + +bool COSMWay::is_node_right(COSMNode &node,bool forward,double tol) +{ + COSMNode *start_node,*end_node; + unsigned int closest_seg; + double angle; + + closest_seg=this->get_closest_segment(node); + if(forward) + { + start_node=&this->get_segment_start_node(closest_seg); + end_node=&this->get_segment_end_node(closest_seg); + angle=end_node->compute_angle(*start_node,node); + if(angle>0.0 && fabs(angle)<(3.14159-tol) && fabs(angle)>tol) + return true; + else + return false; + } + else + { + start_node=&this->get_segment_start_node(closest_seg); + end_node=&this->get_segment_end_node(closest_seg); + angle=start_node->compute_angle(*end_node,node); + if(angle>0.0 && fabs(angle)<(3.14159-tol) && fabs(angle)>tol) + return true; + else + return false; + } +} + +void COSMWay::remove_straight_nodes(void) +{ + std::vector<COSMNode *>::iterator it; + unsigned int i; + double angle; + + if(this->nodes.size()>2) + { + it=this->nodes.begin(); + it++; + for(i=0;i<this->nodes.size()-2;) + { + if(this->get_node_multiplicity(this->nodes[i+1]->id)==1) + { + angle=this->nodes[i+1]->compute_angle(*this->nodes[i],*this->nodes[i+2]); + if(fabs(angle)>=(3.14159-0.01))// straight line + { +// std::cout << "erasing node (ID " << (*it)->id << " of way ID " << this->id << " because it is aligned with the previuous and next ones" << std::endl; + this->parent->delete_node(*it); + it=this->nodes.erase(it); + } + else + { + it++; + i++; + } + } + else + { + it++; + i++; + } + } + } +} + +void COSMWay::add_restriction(COSMRestriction *new_restriction) +{ + for(unsigned int i=0;i<this->restrictions.size();i++) + if(this->restrictions[i]->id==new_restriction->id) + return; + this->restrictions.push_back(new_restriction); +} + +void COSMWay::delete_restriction(COSMRestriction *restriction) +{ + std::vector<COSMRestriction *>::iterator it; + + for(it=this->restrictions.begin();it!=this->restrictions.end();it++) + { + if((*it)==restriction) + { + this->restrictions.erase(it); + break; + } + } +} + +void COSMWay::delete_restriction(long int id) +{ + std::vector<COSMRestriction *>::iterator it; + + for(it=this->restrictions.begin();it!=this->restrictions.end();) + { + if((*it)->get_via_node().get_id()==id) + it=this->restrictions.erase(it); + else + it++; + } +} + +void COSMWay::update_restriction(COSMWay *old_way,COSMWay *new_way) +{ + std::vector<COSMRestriction *>::const_iterator it; + COSMRestriction *new_restriction=NULL; + bool no_u_turn=true; + + for(it=this->restrictions.begin();it!=this->restrictions.end();) + { + if(!this->has_node((*it)->get_via_node().get_id())) + it=this->restrictions.erase(it); + else + it++; + } + for(it=this->restrictions.begin();it!=this->restrictions.end();it++) + { + if(this->id==old_way->get_id()) + { + if(!((*it)->get_action()==RESTRICTION_NO && (*it)->get_type()==RESTRICTION_U_TURN)) + { + if((*it)->get_to_way().get_id()==old_way->get_id()) + (*it)->update_to_way(old_way,new_way); + } + else + no_u_turn=false; + } + else if(this->id==new_way->get_id()) + { + if((*it)->get_action()==RESTRICTION_NO && (*it)->get_type()==RESTRICTION_U_TURN) + { + (*it)->update_from_way(old_way,new_way); + (*it)->update_to_way(old_way,new_way); + no_u_turn=false; + } + else + { + if((*it)->get_from_way().get_id()==old_way->get_id()) + (*it)->update_from_way(old_way,new_way); + } + } + } + /* + if(!no_u_turn) + { + new_restriction= new COSMRestriction(this,,this,RESTRICTION_NO,RESTRICTION_U_TURN,this->parent); + } + if(new_restriction!=NULL) + { + this->parent->add_restriction(new_restriction); + new_way->add_restriction(new_restriction); + new_restriction->via->add_restriction(new_restriction); + } + */ +} + +void COSMWay::clear_restrictions(void) +{ + this->restrictions.clear(); +} + +COSMWay *COSMWay::split(long int node_id) +{ + std::vector<COSMNode *>::iterator node_it,it; + COSMWay *new_way; + + for(node_it=this->nodes.begin();node_it!=this->nodes.end();++node_it) + { + if((*node_it)->id==node_id) + { + // create a new way + new_way=new COSMWay(*this); + new_way->id=-1; + this->parent->add_way(new_way); + // update the current way + node_it++; + this->nodes.erase(node_it,this->nodes.end()); + this->update_restriction(this,new_way); + // update the new way + for(it=new_way->nodes.begin();it!=new_way->nodes.end();++it) + if((*it)->id==node_id) + { + it=new_way->nodes.erase(new_way->nodes.begin(),it); + break; + } + for(it=new_way->nodes.begin()+1;it!=new_way->nodes.end();++it) + (*it)->update_way(this,new_way); + new_way->update_restriction(this,new_way); + return new_way; + } + } + return NULL; +} + +void COSMWay::merge(COSMWay *way) +{ + std::vector<COSMNode *>::iterator it; + + for(it=way->nodes.begin();it!=way->nodes.end();++it) + { + if(it==way->nodes.begin())// remove the reference to the second way + (*it)->delete_way(way); + else + (*it)->update_way(way,this); + } + this->nodes.insert(this->nodes.end(),way->nodes.begin()+1,way->nodes.end()); + for(unsigned int i=0;i<way->restrictions.size();i++) + this->add_restriction(way->restrictions[i]); + for(unsigned int i=0;i<this->restrictions.size();i++) + { + this->restrictions[i]->update_to_way(way,this); + this->restrictions[i]->update_from_way(way,this); + } +} + +bool COSMWay::is_not_connected(void) +{ + for(unsigned int i=0;i<this->nodes.size();i++) + { + if(this->nodes[i]->get_num_ways()>1) + return false; + } + return true; +} + +long int COSMWay::get_id(void) +{ + return this->id; +} + +std::string COSMWay::get_name(void) +{ + return this->name; +} + +unsigned int COSMWay::get_num_nodes(void) +{ + return this->nodes.size(); +} + +COSMNode &COSMWay::get_node_by_index(unsigned int index) +{ + unsigned int i; + std::stringstream text; + std::vector<COSMNode *>::iterator it; + + for(it=this->nodes.begin(),i=0;it!=this->nodes.end();++it,i++) + if(i==index) + return **it; + text << "Way with id " << this->id << " does not have to so many nodes"; + throw CException(_HERE_,text.str()); +} + +COSMNode &COSMWay::get_next_node_by_index(unsigned int index) +{ + unsigned int i; + std::stringstream text; + std::vector<COSMNode *>::iterator it; + + for(it=this->nodes.begin(),i=0;it!=this->nodes.end();++it,i++) + if(i==index) + { + if(i<this->nodes.size()-1) + { + it++; + return **it; + } + text << "Node at index " << index << " is the last one"; + throw CException(_HERE_,text.str()); + } + text << "Way with id " << this->id << " does not have to so many nodes"; + throw CException(_HERE_,text.str()); +} + +COSMNode &COSMWay::get_prev_node_by_index(unsigned int index) +{ + unsigned int i; + std::stringstream text; + std::vector<COSMNode *>::iterator it; + + for(it=this->nodes.begin(),i=0;it!=this->nodes.end();++it,i++) + if(i==index) + { + if(i>0) + { + it--; + return **it; + } + text << "Node at index " << index << " is the first one"; + throw CException(_HERE_,text.str()); + } + text << "Way with id " << this->id << " does not have to so many nodes"; + throw CException(_HERE_,text.str()); +} + +COSMNode &COSMWay::get_node_by_id(long int id) +{ + std::stringstream text; + + for(unsigned int i=0;i<this->nodes.size();i++) + if(this->nodes[i]->id==id) + return *this->nodes[i]; + text << "Way with id " << this-> id << " does not include node with id " << id; + throw CException(_HERE_,text.str()); +} + +COSMNode &COSMWay::get_next_node_by_id(long int id) +{ + std::stringstream text; + + for(unsigned int i=0;i<this->nodes.size();i++) + if(this->nodes[i]->id==id) + { + if(i<this->nodes.size()-1) + return *this->nodes[i+1]; + text << "Node with id " << id << " is the last one"; + throw CException(_HERE_,text.str()); + } + text << "Way with id " << this-> id << " does not include node with id " << id; + throw CException(_HERE_,text.str()); +} + +COSMNode &COSMWay::get_prev_node_by_id(long int id) +{ + std::stringstream text; + + for(unsigned int i=0;i<this->nodes.size();i++) + if(this->nodes[i]->id==id) + { + if(i>0) + return *this->nodes[i-1]; + text << "Node with id " << id << " is the first one"; + throw CException(_HERE_,text.str()); + } + text << "Way with id " << this-> id << " does not include node with id " << id; + throw CException(_HERE_,text.str()); +} + +COSMNode &COSMWay::get_closest_node(COSMNode &node) +{ + unsigned int i=0,min_index=-1; + std::vector<COSMNode *>::iterator it; + double min_dist=std::numeric_limits<double>::max(),dist; + + if(this->nodes.size()>0) + { + for(it=this->nodes.begin(),i=0;it!=this->nodes.end();it++,i++) + { + dist=(*it)->compute_distance(node); + if(dist<min_dist) + { + min_dist=dist; + min_index=i; + } + } + return *this->nodes[min_index]; + } + else + throw CException(_HERE_,"The way has no nodes"); +} + +unsigned int COSMWay::get_node_multiplicity(long int id) +{ + unsigned int num=0; + + for(unsigned int i=0;i<this->nodes.size();i++) + if(this->nodes[i]->id==id) + num++; + + return num; +} + +bool COSMWay::has_node(long int id) +{ + for(unsigned int i=0;i<this->nodes.size();i++) + if(this->nodes[i]->get_id()==id) + return true; + + return false; +} + +unsigned int COSMWay::get_num_lanes(void) +{ + return this->num_forward_lanes+this->num_backward_lanes; +} + +unsigned int COSMWay::get_num_forward_lanes(void) +{ + return this->num_forward_lanes; +} + +unsigned int COSMWay::get_num_backward_lanes(void) +{ + return this->num_backward_lanes; +} + +bool COSMWay::is_one_way(void) +{ + if(this->num_backward_lanes==0) + return true; + else + return false; +} + +unsigned int COSMWay::get_num_segments(void) +{ + return this->nodes.size()-1; +} + +COSMNode &COSMWay::get_segment_start_node(unsigned int index) +{ + std::stringstream text; + + if(index>this->nodes.size()-2 && index!=(unsigned int)-1) + { + text << "Way with id " << this->id << " does not have to so many segments"; + throw CException(_HERE_,text.str()); + } + if(index==(unsigned int)-1) + return *this->nodes[this->nodes.size()-2]; + else + return *this->nodes[index]; +} + +COSMNode &COSMWay::get_segment_end_node(unsigned int index) +{ + std::stringstream text; + + if(index>this->nodes.size()-2 && index!=(unsigned int)-1) + { + text << "Way with id " << this->id << " does not have to so many segments"; + throw CException(_HERE_,text.str()); + } + if(index==(unsigned int)-1) + return *this->nodes[this->nodes.size()-1]; + else + return *this->nodes[index+1]; +} + +unsigned int COSMWay::get_closest_segment(COSMNode &node) +{ + unsigned int i=0,min_index=-1; + std::vector<COSMNode *>::iterator it; + double min_dist=std::numeric_limits<double>::max(),dist,dist_next,dist_prev; + + if(this->nodes.size()>0) + { + for(it=this->nodes.begin(),i=0;it!=this->nodes.end();it++,i++) + { + dist=(*it)->compute_distance(node); + if(dist<min_dist) + { + min_dist=dist; + min_index=i; + } + } + if(min_index==0) + return 0; + else if(min_index==this->nodes.size()-1) + return this->nodes.size()-2; + else + { + dist_next=this->nodes[min_index+1]->compute_distance(node); + dist_prev=this->nodes[min_index-1]->compute_distance(node); + if(dist_prev<dist_next) + return min_index-1; + else + return min_index; + } + } + else + throw CException(_HERE_,"The way has no nodes"); +} + +lane_restructions_t COSMWay::get_lane_restriction(unsigned int index) +{ + if(index>this->get_num_lanes()-1) + throw CException(_HERE_,"The was does not have so many lanes"); + if(index<this->num_forward_lanes) + return this->forward_lanes[index]; + else + return this->backward_lanes[index-this->num_forward_lanes]; +} + +double COSMWay::get_lane_width(void) +{ + if(this->lane_width==-1.0) + return DEFAULT_LANE_WIDTH; + else + return this->lane_width; +} + +COSMRoadSegment &COSMWay::get_road_segment(void) +{ + return *this->road_segment; +} + +COSMMap &COSMWay::get_parent(void) +{ + return *this->parent; +} + +std::ostream& operator<<(std::ostream& out, COSMWay &way) +{ + out << " id: " << way.get_id() << std::endl; + out << " num. forward lanes: " << way.get_num_forward_lanes() << std::endl; + out << " num. backward lanes: " << way.get_num_backward_lanes() << std::endl; + out << " lane width: " << way.get_lane_width() << std::endl; + out << " number of nodes: " << way.get_num_nodes() << std::endl; + for(unsigned int i=0;i<way.get_num_nodes();i++) + out << " node id: " << way.get_node_by_index(i).get_id() << std::endl; + + return out; +} + +COSMWay::~COSMWay() +{ + this->nodes.clear(); + this->num_forward_lanes=0; + this->num_backward_lanes=0; +} + diff --git a/src/road.cpp b/src/road.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c7c1f090c90e352be6962be07a7a1ae616db399a --- /dev/null +++ b/src/road.cpp @@ -0,0 +1,455 @@ +#include "road.h" +#include "exceptions.h" + +#include <limits> + +CRoad::CRoad() +{ + this->id=-1; + this->resolution=0.1; + this->parent_roadmap=NULL; + this->prev_junction=NULL; + this->next_junction=NULL; + this->opposite_direction_road=NULL; + this->segments.clear(); + this->num_lanes=0; + this->lane_width=0.0; + this->lane_speed=0.0; + this->start_point.x=std::numeric_limits<double>::max(); +} + +CRoad::CRoad(unsigned int id) +{ + this->set_id(id); + this->resolution=0.1; + this->parent_roadmap=NULL; + this->prev_junction=NULL; + this->next_junction=NULL; + this->opposite_direction_road=NULL; + this->segments.clear(); + this->num_lanes=0; + this->lane_width=0.0; + this->lane_speed=0.0; + this->start_point.x=std::numeric_limits<double>::max(); +} + +void CRoad::set_id(unsigned int id) +{ + this->id=id; +} + +unsigned int CRoad::get_index_by_id(const std::vector<CRoad *> &roads,unsigned int id) +{ + std::vector<CRoad *>::const_iterator it; + unsigned int index; + + for(it=roads.begin(),index=0;it!=roads.end();it++,index++) + if((*it)->id==id) + return index; + + return -1; +} + +unsigned int CRoad::get_id(void) +{ + return this->id; +} + +CRoad::~CRoad() +{ + this->id=-1; + this->parent_roadmap=NULL; + this->prev_junction=NULL; + this->next_junction=NULL; + this->opposite_direction_road=NULL; + for(unsigned int i=0;i<this->segments.size();i++) + { + if(this->segments[i]!=NULL) + { + delete this->segments[i]; + this->segments[i]=NULL; + } + } + this->segments.clear(); +} + +/* connectivity */ +void CRoad::set_parent_roadmap(CRoadMap *roadmap) +{ + if(roadmap==NULL) + throw CException(_HERE_,"Invalid roadmap reference"); + this->parent_roadmap=roadmap; +} + +void CRoad::set_prev_junction(CJunction *junction) +{ + this->prev_junction=junction; +} + +void CRoad::set_next_junction(CJunction *junction) +{ + this->next_junction=junction; +} + +void CRoad::set_opposite_direction_road(CRoad *road) +{ + if(road==NULL) + throw CException(_HERE_,"Invalid opposite direction road reference"); + this->opposite_direction_road=road; +} + +/* geometry */ +CRoadSegment *CRoad::get_first_segment(void) +{ + if(this->segments.size()==0) + throw CException(_HERE_,"Road has no segments"); + return this->segments[0]; +} + +CRoadSegment *CRoad::get_last_segment(void) +{ + if(this->segments.size()==0) + throw CException(_HERE_,"Road has no segments"); + return this->segments[this->segments.size()-1]; +} + +void CRoad::set_resolution(double resolution) +{ + this->resolution=resolution; + for(unsigned int i=0;i<this->segments.size();i++) + this->segments[i]->set_resolution(resolution); +} + +double CRoad::get_resolution(void) +{ + return this->resolution; +} + +CRoadMap &CRoad::get_parent_roadmap(void) +{ + if(this->parent_roadmap==NULL) + throw CException(_HERE_,"No parent road map has been defined"); + return *this->parent_roadmap; +} + +bool CRoad::exist_prev_junction(void) +{ + if(this->prev_junction!=NULL) + return true; + else + return false; +} + +CJunction &CRoad::get_prev_junction(void) +{ + if(this->prev_junction==NULL) + throw CException(_HERE_,"No previous junction has been defined"); + return *this->prev_junction; +} + +bool CRoad::exist_next_junction(void) +{ + if(this->next_junction!=NULL) + return true; + else + return false; +} + +CJunction &CRoad::get_next_junction(void) +{ + if(this->next_junction==NULL) + throw CException(_HERE_,"No next junction has been defined"); + return *this->next_junction; +} + +bool CRoad::has_opposite_direction_road(void) +{ + if(this->opposite_direction_road!=NULL) + return true; + else + return false; +} + +CRoad &CRoad::get_opposite_direction_road(void) +{ + if(this->opposite_direction_road==NULL) + throw CException(_HERE_,"No opposite direction road has been defined"); + return *this->opposite_direction_road; +} + +/* geometry */ +void CRoad::set_num_lanes(unsigned int num) +{ + this->num_lanes=num; +} + +unsigned int CRoad::get_num_lanes(void) +{ + return this->num_lanes; +} + +void CRoad::set_lane_width(double width) +{ + if(width<0.0) + throw CException(_HERE_,"Lane width can not be negative"); + this->lane_width=width; +} + +double CRoad::get_lane_width(void) +{ + return this->lane_width; +} + +void CRoad::set_lane_speed(double speed) +{ + if(speed<0.0) + throw CException(_HERE_,"Lane speed can not be negative"); + this->lane_speed=speed; +} + +double CRoad::get_lane_speed(void) +{ + return this->lane_speed; +} + + +unsigned int CRoad::get_num_segments(void) +{ + return this->segments.size(); +} + +CRoadSegment *CRoad::get_segment_by_index(unsigned int index) +{ + if(index<0 || index >= this->segments.size()) + throw CException(_HERE_,"Invalid segment index"); + return this->segments[index]; +} + +CRoadSegment &CRoad::get_segment_by_id(unsigned int id) +{ + unsigned int index; + + index=CRoadSegment::get_index_by_id(this->segments,id); + return *this->get_segment_by_index(index); +} + +void CRoad::set_start_point(TPoint &start_point) +{ + TPoint end_point; + + this->start_point=start_point; + if(this->segments.size()>0) + { + this->segments[0]->get_end_point(end_point); + this->segments[0]->set_geometry(this->start_point,end_point); + } +} + +void CRoad::set_start_point(double x,double y,double heading, double curvature) +{ + TPoint point; + + point.x=x; + point.y=y; + point.heading=heading; + point.curvature=curvature; + this->set_start_point(point); +} + +void CRoad::add_segment(TPoint &end_point) +{ + CRoadSegment *new_segment; + TPoint start_point; + + if(this->start_point.x==std::numeric_limits<double>::max()) + throw CException(_HERE_,"The start position has not been defined"); + if(this->num_lanes<1) + throw CException(_HERE_,"Road has no lanes"); + if(this->segments.size()==0) + start_point=this->start_point; + else + this->segments[this->segments.size()-1]->get_end_point(start_point); + new_segment=new CRoadSegment(this->num_lanes,this->num_lanes); + new_segment->set_resolution(this->resolution); + new_segment->set_geometry(start_point,end_point); + new_segment->set_parent_road(this); + if(this->segments.size()>0) + { + this->segments[this->segments.size()-1]->add_next_segment(new_segment); + new_segment->add_prev_segment(this->segments[this->segments.size()-1]); + if(this->parent_roadmap!=NULL) + new_segment->id=this->parent_roadmap->get_next_segment_id(); + } + new_segment->set_full_connectivity(); + this->segments.push_back(new_segment); +} + +void CRoad::add_segment(double x,double y,double heading, double curvature) +{ + TPoint point; + + point.x=x; + point.y=y; + point.heading=heading; + point.curvature=curvature; + this->add_segment(point); +} + +void CRoad::remove_segment_by_index(unsigned int index) +{ + std::vector<CRoadSegment *>::iterator it; + TPoint end_point,start_point; + unsigned int i; + + if(index<0 || index >= this->segments.size()) + throw CException(_HERE_,"Invalid segment index"); + if(this->segments.size()==1) + throw CException(_HERE_,"Thr road will have no geometry after removing the segment"); + for(it=this->segments.begin(),i=0;it!=this->segments.end();it++,i++) + { + if(index==i) + { + (*it)->get_end_point(end_point); + for(unsigned int i=0;i<(*it)->get_num_prev_segments();i++) + { + (*it)->prev_segments[i]->get_start_point(start_point); + (*it)->prev_segments[i]->set_geometry(start_point,end_point); + (*it)->prev_segments[i]->next_segments=(*it)->next_segments; + } + for(unsigned int i=0;i<(*it)->get_num_next_segments();i++) + (*it)->next_segments[i]->prev_segments=(*it)->prev_segments; + delete *it; + this->segments.erase(it); + break; + } + } +} + +void CRoad::merge(CRoad *road) +{ + TPoint end_point; + + if(road==NULL) + throw CException(_HERE_,"Invalid road reference"); + for(unsigned int i=0;i<road->get_num_segments();i++) + { + road->segments[i]->get_end_point(end_point); + this->add_segment(end_point); + this->get_last_segment()->connectivity=road->segments[i]->connectivity; + } +} + +bool CRoad::get_point_at(double distance, double lateral_offset,TPoint &point) +{ + for(unsigned int i=0;i<this->segments.size();i++) + { + if(this->segments[i]->get_point_at(distance,lateral_offset,point)) + return true; + else + distance-=this->segments[i]->get_length(); + } + return false; +} + +bool CRoad::get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol) +{ + double min_offset=std::numeric_limits<double>::max(),tmp_offset,tmp_distance; + TPoint tmp_point; + + for(unsigned int i=0;i<this->segments.size();i++) + { + if(this->segments[i]->get_closest_point(target_point,tmp_point,tmp_distance,tmp_offset,angle_tol)) + { + if(fabs(tmp_offset)<min_offset) + { + min_offset=fabs(tmp_offset); + closest_point=tmp_point; + distance=tmp_distance; + lateral_offset=tmp_offset; + } + } + } + if(min_offset!=std::numeric_limits<double>::max()) + return true; + else + return false; +} + +bool CRoad::get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol) +{ + double min_offset=std::numeric_limits<double>::max(),tmp_offset,tmp_distance; + TPoint tmp_point; + + for(unsigned int i=0;i<this->segments.size();i++) + { + if(this->segments[i]->get_closest_point(point,tmp_point,tmp_distance,tmp_offset,angle_tol)) + { + if(fabs(tmp_offset)<min_offset) + { + min_offset=fabs(tmp_offset); + distance=tmp_distance; + lateral_offset=tmp_offset; + segment_id=this->segments[i]->get_id(); + } + } + } + if(min_offset!=std::numeric_limits<double>::max()) + return true; + else + return false; +} + +bool CRoad::get_segment_id_at(double distance,unsigned int &segment_id) +{ + for(unsigned int i=0;i<this->segments.size();i++) + { + if(distance<this->segments[i]->get_length()) + { + segment_id=this->segments[i]->get_length(); + return true; + } + else + distance-=this->segments[i]->get_length(); + } + return false; +} + +void CRoad::get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw) +{ + std::vector<double> partial_x,partial_y,partial_heading; + + x.clear(); + y.clear(); + yaw.clear(); + for(unsigned int i=0;i<this->segments.size();i++) + { + this->segments[i]->get_geometry(partial_x,partial_y,partial_heading); + x.insert(x.end(),partial_x.begin(),partial_x.end()); + y.insert(y.end(),partial_y.begin(),partial_y.end()); + yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); + } +} + +void CRoad::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw) +{ + std::vector<double> partial_x,partial_y,partial_heading; + double lateral_offset; + + x.clear(); + y.clear(); + yaw.clear(); + for(unsigned int i=0;i<this->segments.size();i++) + { + lateral_offset=-this->lane_width/2.0; + for(unsigned int j=0;j<this->num_lanes;j++) + { + this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,lateral_offset); + x.insert(x.end(),partial_x.begin(),partial_x.end()); + y.insert(y.end(),partial_y.begin(),partial_y.end()); + yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); + lateral_offset-=this->lane_width; + } + } +} + + diff --git a/src/road_map.cpp b/src/road_map.cpp new file mode 100644 index 0000000000000000000000000000000000000000..dd9e4bb5e419032536bfee23ac818834ba085844 --- /dev/null +++ b/src/road_map.cpp @@ -0,0 +1,893 @@ +#include "road_map.h" +#include "exceptions.h" + +#include "opendrive/opendrive_road_segment.h" +#include "opendrive/opendrive_junction.h" + +#include "osm/osm_map.h" + +#include <sys/types.h> +#include <sys/stat.h> +#include <chrono> +#include <fstream> + +CRoadMap::CRoadMap() +{ + this->resolution=0.1; + this->next_segment_id=0; + this->next_road_id=0; + this->next_junction_id=0; +} + +void CRoadMap::free(void) +{ + for(unsigned int i=0;i<this->roads.size();i++) + if(this->roads[i]!=NULL) + { + delete this->roads[i]; + this->roads[i]=NULL; + } + this->roads.clear(); + for(unsigned int i=0;i<this->junctions.size();i++) + if(this->junctions[i]!=NULL) + { + delete this->junctions[i]; + this->junctions[i]=NULL; + } + this->junctions.clear(); + this->next_segment_id=0; + this->next_road_id=0; + this->next_junction_id=0; +} + +unsigned int CRoadMap::get_next_segment_id(void) +{ + this->next_segment_id++; + + return this->next_segment_id-1; +} + +unsigned int CRoadMap::get_next_road_id(void) +{ + this->next_road_id++; + + return this->next_road_id-1; +} + +unsigned int CRoadMap::get_next_junction_id(void) +{ + this->next_junction_id++; + + return this->next_junction_id-1; +} + +CRoad *CRoadMap::get_road_by_index(unsigned int index) +{ + if(index<0 || index >= this->roads.size()) + throw CException(_HERE_,"Invalid road index"); + return this->roads[index]; +} + +CJunction *CRoadMap::get_junction_by_index(unsigned int index) +{ + if(index<0 || index >= this->junctions.size()) + throw CException(_HERE_,"Invalid junctions index"); + return this->junctions[index]; +} + +void CRoadMap::get_segment_by_id(unsigned int id,CRoadSegment **segment) +{ + for(unsigned int i=0;i<this->roads.size();i++) + for(unsigned int j=0;j<this->roads[i]->get_num_segments();j++) + if(this->roads[i]->segments[j]->get_id()==id) + { + (*segment)=this->roads[i]->segments[j]; + return; + } + for(unsigned int i=0;i<this->junctions.size();i++) + for(unsigned int j=0;j<this->junctions[i]->get_num_segments();j++) + if(this->junctions[i]->segments[j]->get_id()==id) + { + (*segment)=this->junctions[i]->segments[j]; + return; + } + throw CException(_HERE_,"No segment with the given id"); +} + +void CRoadMap::remove_road_by_index(unsigned int index) +{ + std::vector<CRoad *>::iterator it; + unsigned int i; + + if(index<0 || index >= this->roads.size()) + throw CException(_HERE_,"Invalid road index"); + for(it=this->roads.begin(),i=0;it!=this->roads.end();it++,i++) + { + if(i==index) + { + /* update connectivity information */ + if((*it)->exist_prev_junction()) + (*it)->prev_junction->remove_outgoing_road_by_id((*it)->id); + if((*it)->exist_next_junction()) + (*it)->next_junction->remove_incomming_road_by_id((*it)->id); + delete *it; + this->roads.erase(it); + break; + } + } +} + +void CRoadMap::set_opposite_direction_roads_by_index(unsigned int right_road,unsigned int left_road) +{ + CRoad *left,*right; + CRoadSegment *left_segment,*right_segment; + TPoint left_point,right_point; + + if(right_road<0 || right_road >= this->roads.size()) + throw CException(_HERE_,"Invalid right road index"); + if(left_road<0 || left_road >= this->roads.size()) + throw CException(_HERE_,"Invalid left road index"); + + left=this->get_road_by_index(left_road); + right=this->get_road_by_index(right_road); + // check geometry + if(left->get_num_segments()!=right->get_num_segments()) + throw CException(_HERE_,"The number of segments in both roads do not match"); + for(unsigned int i=0;i<right->get_num_segments();i++) + { + left_segment=left->get_segment_by_index(left->get_num_segments()-1-i); + left_segment->get_start_point(left_point); + right_segment=right->get_segment_by_index(i); + right_segment->get_end_point(right_point); + if(sqrt(pow(right_point.x-left_point.x,2.0)+pow(right_point.y-left_point.y,2.0))>this->resolution) + throw CException(_HERE_,"Intermediate geometry points do not match"); + } + left->set_opposite_direction_road(right); + right->set_opposite_direction_road(left); +} + +void CRoadMap::remove_junction_by_index(unsigned int index) +{ + std::vector<CJunction *>::iterator it; + unsigned int i; + + if(index<0 || index >= this->junctions.size()) + throw CException(_HERE_,"Invalid junction index"); + for(it=this->junctions.begin(),i=0;it!=this->junctions.end();it++,i++) + { + if(i==index) + { + /* update connectivity information */ + for(unsigned int i=0;i<(*it)->get_num_incomming_roads();i++) + (*it)->get_incomming_road_by_index(i)->set_next_junction(NULL); + for(unsigned int i=0;i<(*it)->get_num_outgoing_roads();i++) + (*it)->get_outgoing_road_by_index(i)->set_prev_junction(NULL); + delete *it; + this->junctions.erase(it); + break; + } + } +} + +void CRoadMap::merge_roads(opendrive_road_map_t &road_map) +{ + CRoad *del_road; + + for(opendrive_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++) + { + if(it->first->has_successor()) + { + if(it->first->is_successor_road()) + { + for(opendrive_road_map_t::iterator it2=road_map.begin();it2!=road_map.end();it2++) + { + if(it2->first->get_id()==it->first->get_successor_id()) + { + if(it->second.size()>0 && it2->second.size()>0) + { + (it->second)[0]->merge((it2->second)[0]); + del_road=(it2->second)[0]; + delete (it2->second)[0]; + for(opendrive_road_map_t::iterator it3=road_map.begin();it3!=road_map.end();it3++) + if((it3->second)[0]==del_road) + (it3->second)[0]=(it->second)[0]; + } + if(it->second.size()>1 && it2->second.size()>1) + { + (it2->second)[1]->merge((it->second)[1]); + del_road=(it->second)[1]; + delete (it->second)[1]; + for(opendrive_road_map_t::iterator it3=road_map.begin();it3!=road_map.end();it3++) + if((it3->second)[1]==del_road) + (it3->second)[1]=(it2->second)[1]; + (it2->second)[1]->set_opposite_direction_road((it->second)[0]); + (it2->second)[0]->set_opposite_direction_road((it->second)[1]); + } + break; + } + } + } + } + } +} + +void CRoadMap::load_opendrive(const std::string &filename) +{ + struct stat buffer; + COpendriveRoadSegment *segment; + COpendriveJunction *junction; + CRoad *left_road,*right_road; + CJunction *new_junction; + opendrive_road_map_t road_map; + opendrive_map_pair_t map_pair; + std::vector<CRoad *> segment_roads; + + this->free(); + if(stat(filename.c_str(),&buffer)==0) + { + try{ + std::unique_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate)); + /* load roads */ + for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it) + { + try{ + segment=new COpendriveRoadSegment(); + segment->load(*road_it); + if(!segment->is_junction()) + { + segment->convert(&left_road,&right_road,this->resolution); + segment_roads.clear(); + if(right_road!=NULL) + segment_roads.push_back(right_road); + if(left_road!=NULL) + segment_roads.push_back(left_road); + map_pair=std::make_pair(segment,segment_roads); + road_map.push_back(map_pair); + } + }catch(CException &e){ + for(opendrive_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++) + { + delete it->first; + for(unsigned int i=0;i<it->second.size();i++) + delete it->second[i]; + } + std::cout << e.what() << std::endl; + } + } + /* merge consecutive roads */ + this->merge_roads(road_map); + /* add all roads */ + for(opendrive_road_map_t::iterator it=road_map.begin();it!=road_map.end();it++) + { + for(unsigned int i=0;i<it->second.size();i++) + { + try{ + this->add_road((it->second)[i]); + }catch(CException &e){ + } + } + } + /* load junctions */ + for (OpenDRIVE::junction_iterator junction_it(open_drive->junction().begin()); junction_it != open_drive->junction().end(); ++junction_it) + { + try{ + junction=new COpendriveJunction(); + junction->load(*junction_it); + junction->convert(&new_junction,road_map); + if(new_junction!=NULL) + this->add_junction(new_junction); + delete junction; + }catch(CException &e){ + std::cout << e.what() << std::endl; + } + } + /* free all opendrive segments */ + for(unsigned int i=0;i<road_map.size();i++) + delete road_map[i].first; + }catch (const xml_schema::exception& e){ + this->free(); + std::ostringstream os; + os << e; + /* handle exceptions */ + throw CException(_HERE_,os.str()); + } + } + else + throw CException(_HERE_,"The .xodr file does not exist"); +} + +void CRoadMap::save_opendrive(const std::string &filename) +{ + std::vector<CRoad *> processed_roads; + opendrive_inverse_map_pair_t map_pair; + opendrive_road_inverse_map_t road_map; + CRoad *opposite_road; + std::vector<COpendriveRoadSegment *> junction_segments,segments; + COpendriveJunction *junction; + xml_schema::namespace_infomap map; + ::header opendrive_header; + ::OpenDRIVE *opendrive_data; + OpenDRIVE::road_type *new_road; + unsigned int road_id=0; + + try{ + std::ofstream output_file(filename.c_str(),std::ios_base::out); + opendrive_header.revMajor(1.0); + opendrive_header.revMinor(1.0); + opendrive_header.name("iri"); + opendrive_header.version(1.0); + opendrive_header.north(0.0); + opendrive_header.south(0.0); + opendrive_header.east(0.0); + opendrive_header.west(0.0); + std::time_t current_time = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + opendrive_header.date(std::ctime(¤t_time)); + opendrive_data=new ::OpenDRIVE(opendrive_header); + for(unsigned int i=0;i<this->roads.size();i++) + { + if(std::find(processed_roads.begin(),processed_roads.end(),this->roads[i])==processed_roads.end()) + { + map_pair.first.clear(); + map_pair.second.clear(); + processed_roads.push_back(this->roads[i]); + map_pair.first.push_back(this->roads[i]); + if(this->roads[i]->has_opposite_direction_road()) + { + opposite_road=&this->roads[i]->get_opposite_direction_road(); + processed_roads.push_back(opposite_road); + map_pair.first.push_back(opposite_road); + } + else + opposite_road=NULL; + COpendriveRoadSegment::convert(opposite_road,this->roads[i],road_id,segments); + for(unsigned j=0;j<segments.size();j++) + { + segments[j]->save(&new_road); + opendrive_data->road().push_back(*new_road); + delete new_road; + map_pair.second.push_back(segments[j]); + } + road_map.push_back(map_pair); + } + } + for(unsigned int i=0;i<this->junctions.size();i++) + { + junction=new COpendriveJunction(); + junction->convert(this->junctions[i],road_id,junction,junction_segments,road_map); + OpenDRIVE::junction_type new_junction; + junction->save(new_junction); + opendrive_data->junction().push_back(new_junction); + for(unsigned int j=0;j<junction_segments.size();j++) + { + junction_segments[j]->save(&new_road); + opendrive_data->road().push_back(*new_road); + delete new_road; + delete junction_segments[j]; + } + delete junction; + } + for(unsigned int i=0;i<road_map.size();i++) + for(unsigned int j=0;j<road_map[i].second.size();j++) + delete road_map[i].second[j]; + map[""].name=""; + map[""].schema="OpenDRIVE_1.4H.xsd"; + OpenDRIVE_(output_file,*opendrive_data,map); + }catch (const xml_schema::exception& e){ + std::ostringstream os; + os << e; + /* handle exceptions */ + throw CException(_HERE_,os.str()); + } +} + +void CRoadMap::load_osm(const std::string &filename) +{ + COSMMap road_map; + + this->free(); + road_map.load(filename); + road_map.convert(*this); +} + +void CRoadMap::set_resolution(double resolution) +{ + this->resolution=resolution; + for(unsigned int i=0;i<this->roads.size();i++) + this->roads[i]->set_resolution(resolution); + for(unsigned int i=0;i<this->junctions.size();i++) + this->junctions[i]->set_resolution(resolution); +} + +double CRoadMap::get_resolution(void) +{ + return this->resolution; +} + +std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned int> &segment_ids,CRoadMap &new_road_map) +{ + std::map<CRoad *,CRoad *> road_ref_update; + std::map<CRoadSegment *,CRoadSegment *> segment_ref_update; + std::vector<CRoadSegment *> junction_segments; + CRoad *new_road=NULL,*old_parent_road; + CJunction *new_junction; + CRoadSegment *new_segment,*old_segment; + TPoint start_point,end_point; + unsigned int next_id,prev_id,num_lanes_in,num_lanes_out; + std::vector<unsigned int> new_path; + + new_road_map.free(); + for(unsigned int i=0;i<segment_ids.size();i++) + { + old_segment=&this->get_segment_by_id(segment_ids[i]); + if(old_segment->has_parent_road())// parent is a road + { + if(new_road==NULL) + { + new_road=new CRoad(); + old_parent_road=&old_segment->get_parent_road(); + road_ref_update[old_parent_road]=new_road; + new_road->set_resolution(old_parent_road->get_resolution()); + new_road->set_num_lanes(old_parent_road->get_num_lanes()); + new_road->set_lane_width(old_parent_road->get_lane_width()); + new_road->set_lane_speed(old_parent_road->get_lane_speed()); + old_segment->get_start_point(start_point); + new_road->set_start_point(start_point); + old_segment->get_end_point(end_point); + new_road->add_segment(end_point); + } + else + { + old_segment->get_end_point(end_point); + new_road->add_segment(end_point); + } + new_segment=new_road->get_last_segment(); + new_segment->connectivity=old_segment->connectivity; + segment_ref_update[old_segment]=new_segment; + } + else // parent is a junction + { + if(new_road!=NULL) + { + new_road_map.add_road(new_road); + new_road=NULL; + } + junction_segments.push_back(old_segment); + } + } + if(new_road!=NULL) + { + new_road_map.add_road(new_road); + new_road=NULL; + } + //handle the junctions + for(unsigned int i=0;i<junction_segments.size();i++) + { + if(junction_segments[i]->get_num_prev_segments()==1) + { + if(junction_segments[i]->get_prev_segment_by_index(0).has_parent_road()) + { + if(road_ref_update.find(&junction_segments[i]->get_prev_segment_by_index(0).get_parent_road())!=road_ref_update.end()) + prev_id=road_ref_update[&junction_segments[i]->get_prev_segment_by_index(0).get_parent_road()]->get_id(); + else + { + prev_id=-1; + junction_segments[i]->get_prev_segment_by_index(0).get_end_point(start_point); + num_lanes_in=junction_segments[i]->get_prev_segment_by_index(0).get_num_lanes_out(); + } + } + else + continue; + } + else + continue; + if(junction_segments[i]->get_num_next_segments()==1) + { + if(junction_segments[i]->get_next_segment_by_index(0).has_parent_road()) + { + if(road_ref_update.find(&junction_segments[i]->get_next_segment_by_index(0).get_parent_road())!=road_ref_update.end()) + next_id=road_ref_update[&junction_segments[i]->get_next_segment_by_index(0).get_parent_road()]->get_id(); + else + { + next_id=-1; + junction_segments[i]->get_next_segment_by_index(0).get_start_point(end_point); + num_lanes_out=junction_segments[i]->get_next_segment_by_index(0).get_num_lanes_in(); + } + } + else + continue; + } + else + continue; + new_junction=new CJunction(); + if(prev_id!=(unsigned int)-1) + new_junction->add_incomming_road(&new_road_map.get_road_by_id(prev_id)); + if(next_id!=(unsigned int)-1) + new_junction->add_outgoing_road(&new_road_map.get_road_by_id(next_id)); + if(prev_id==(unsigned int)-1) + { + if(next_id==(unsigned int)-1) + new_segment=new_junction->link_point_to_point(start_point,num_lanes_in,end_point,num_lanes_out); + else + new_segment=new_junction->link_point_to_road(start_point,num_lanes_in,next_id); + } + else + { + if(next_id==(unsigned int)-1) + new_segment=new_junction->link_road_to_point(prev_id,end_point,num_lanes_out); + else + { + new_junction->link_roads_by_id(prev_id,next_id); + new_segment=&new_junction->get_segment_between_by_id(prev_id,next_id); + } + } + new_segment->connectivity=junction_segments[i]->connectivity; + segment_ref_update[junction_segments[i]]=new_segment; + new_road_map.add_junction(new_junction); + } + for(unsigned int i=0;i<segment_ids.size();i++) + { + for(std::map<CRoadSegment *,CRoadSegment *>::iterator it=segment_ref_update.begin();it!=segment_ref_update.end();it++) + { + if(it->first->get_id()==segment_ids[i]) + { + new_path.push_back(it->second->get_id()); + break; + } + } + } + + return new_path; +} + +double CRoadMap::get_path_length(std::vector<unsigned int> &segment_ids) +{ + CRoadSegment *segment; + double length=0.0; + + for(unsigned int i=0;i<segment_ids.size();i++) + { + segment=&this->get_segment_by_id(segment_ids[i]); + length+=segment->get_length(); + } + + return length; +} + +unsigned int CRoadMap::get_num_roads(void) +{ + return this->roads.size(); +} + +CRoad &CRoadMap::get_road_by_id(unsigned int id) +{ + unsigned int index; + + index=CRoad::get_index_by_id(this->roads,id); + return *this->get_road_by_index(index); +} + +unsigned int CRoadMap::get_num_junctions(void) +{ + return this->junctions.size(); +} + +CJunction &CRoadMap::get_junction_by_id(unsigned int id) +{ + unsigned int index; + + index=CJunction::get_index_by_id(this->junctions,id); + return *this->get_junction_by_index(index); +} + +unsigned int CRoadMap::get_num_segments(void) +{ + unsigned int num_segments=0; + + for(unsigned int i=0;i<this->roads.size();i++) + num_segments+=this->roads[i]->get_num_segments(); + for(unsigned int i=0;i<this->junctions.size();i++) + num_segments+=this->junctions[i]->get_num_segments(); + return num_segments; +} + +CRoadSegment &CRoadMap::get_segment_by_id(unsigned int id) +{ + for(unsigned int i=0;i<this->roads.size();i++) + for(unsigned int j=0;j<this->roads[i]->get_num_segments();j++) + if(this->roads[i]->segments[j]->get_id()==id) + return *this->roads[i]->segments[j]; + for(unsigned int i=0;i<this->junctions.size();i++) + for(unsigned int j=0;j<this->junctions[i]->get_num_segments();j++) + if(this->junctions[i]->segments[j]->get_id()==id) + return *this->junctions[i]->segments[j]; + throw CException(_HERE_,"No segment with the given id"); +} + +void CRoadMap::add_road(CRoad *road) +{ + if(road==NULL) + throw CException(_HERE_,"Invalid road reference"); + if(road->get_id()==(unsigned int)-1) + road->set_id(this->get_next_road_id()); + if(this->has_road(road->get_id())) + throw CException(_HERE_,"Road already present"); + road->set_resolution(this->resolution); + for(unsigned int i=0;i<road->get_num_segments();i++) + if(road->segments[i]->id==(unsigned int)-1) + road->segments[i]->id=this->get_next_segment_id(); + this->roads.push_back(road); + road->set_parent_roadmap(this); +} + +bool CRoadMap::has_road(unsigned int id) +{ + unsigned index; + + index=CRoad::get_index_by_id(this->roads,id); + if(index==(unsigned int)-1) + return false; + else + return true; +} + +void CRoadMap::remove_road_by_id(unsigned int id) +{ + unsigned int index; + + index=CRoad::get_index_by_id(this->roads,id); + this->remove_road_by_index(index); +} + +void CRoadMap::set_opposite_direction_roads_by_id(unsigned int right_road,unsigned int left_road) +{ + unsigned int left_index,right_index; + + right_index=CRoad::get_index_by_id(this->roads,right_road); + left_index=CRoad::get_index_by_id(this->roads,left_road); + this->set_opposite_direction_roads_by_index(right_index,left_index); +} + +void CRoadMap::add_junction(CJunction *junction) +{ + if(junction==NULL) + throw CException(_HERE_,"Invalid junction reference"); + if(junction->get_id()==(unsigned int)-1) + junction->set_id(this->get_next_junction_id()); + if(this->has_junction(junction->get_id())) + throw CException(_HERE_,"Junction already present"); + junction->set_resolution(this->resolution); + for(unsigned int i=0;i<junction->get_num_segments();i++) + if(junction->segments[i]->id==(unsigned int)-1) + junction->segments[i]->id=this->get_next_segment_id(); + this->junctions.push_back(junction); + junction->set_parent_roadmap(this); +} + +bool CRoadMap::has_junction(unsigned int id) +{ + unsigned int index; + + index=CJunction::get_index_by_id(this->junctions,id); + if(index==(unsigned int)-1) + return false; + else + return true; +} + +void CRoadMap::remove_junction_by_id(unsigned int id) +{ + unsigned int index; + + index=CRoad::get_index_by_id(this->roads,id); + this->remove_junction_by_index(index); +} + +void CRoadMap::get_road_connectivity(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &id_map) +{ + connectivity=Eigen::MatrixXi::Zero(this->roads.size(),this->roads.size()); + + id_map.clear(); + for(unsigned int i=0;i<this->roads.size();i++) + { + id_map.push_back(this->roads[i]->get_id()); + for(unsigned int j=0;j<this->roads.size();j++) + { + for(unsigned int k=0;k<this->junctions.size();k++) + { + try{ + if(this->junctions[k]->are_roads_linked_by_id(this->roads[i]->get_id(),this->roads[j]->get_id())) + { + connectivity(i,j)=1; + } + }catch(CException &e){ + /* roads i and j are not connected throw junction k */ + } + } + } + } +} + +void CRoadMap::get_segment_connectivity(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &id_map) +{ + unsigned int num_segments=0,col_index; + std::vector<CRoadSegment *> segments; + + id_map.clear(); + for(unsigned int i=0;i<this->roads.size();i++) + { + num_segments+=this->roads[i]->get_num_segments(); + for(unsigned int j=0;j<this->roads[i]->get_num_segments();j++) + { + id_map.push_back(this->roads[i]->segments[j]->get_id()); + segments.push_back(this->roads[i]->segments[j]); + } + } + for(unsigned int i=0;i<this->junctions.size();i++) + { + num_segments+=this->junctions[i]->get_num_segments(); + for(unsigned int j=0;j<this->junctions[i]->get_num_segments();j++) + { + id_map.push_back(this->junctions[i]->segments[j]->get_id()); + segments.push_back(this->junctions[i]->segments[j]); + } + } + connectivity=Eigen::MatrixXi::Zero(num_segments,num_segments); + for(unsigned int i=0;i<segments.size();i++) + { + for(unsigned int j=0;j<segments[i]->get_num_next_segments();j++) + { + col_index=CRoadSegment::get_index_by_id(segments,segments[i]->next_segments[j]->get_id()); + connectivity(i,col_index)=1; + } + } +} + +void CRoadMap::get_lane_connectivity(Eigen::MatrixXi &connectivity,std::vector<TLaneID> &id_map) +{ + +} + +unsigned int CRoadMap::get_max_num_lanes(void) +{ + unsigned max_num_lanes=0,num_lanes; + + for(unsigned int i=0;i<this->roads.size();i++) + { + num_lanes=this->roads[i]->get_num_lanes(); + if(num_lanes>max_num_lanes) + max_num_lanes=num_lanes; + } + + return max_num_lanes; +} + +bool CRoadMap::get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol) +{ + double min_offset=std::numeric_limits<double>::max(),tmp_offset,tmp_distance; + unsigned int tmp_id; + + for(unsigned int i=0;i<this->roads.size();i++) + { + if(this->roads[i]->get_closest_segment_id(point,tmp_id,tmp_distance,tmp_offset,angle_tol)) + { + if(fabs(tmp_offset)<min_offset) + { + min_offset=fabs(tmp_offset); + distance=tmp_distance; + lateral_offset=tmp_offset; + segment_id=tmp_id; + } + } + } + for(unsigned int i=0;i<this->junctions.size();i++) + { + if(this->junctions[i]->get_closest_segment_id(point,tmp_id,tmp_distance,tmp_offset,angle_tol)) + { + if(fabs(tmp_offset)<min_offset) + { + min_offset=fabs(tmp_offset); + distance=tmp_distance; + lateral_offset=tmp_offset; + segment_id=tmp_id; + } + } + } + + if(min_offset!=std::numeric_limits<double>::max()) + return true; + else + return false; +} + +bool CRoadMap::get_closest_segment_ids(TPoint &point,std::vector<unsigned int> &segment_ids,std::vector<double >&distances,std::vector<double >&lateral_offsets,double angle_tol,double offset_tol) +{ + std::vector<unsigned int> tmp_ids; + std::vector<double> tmp_distances,tmp_offsets; + double tmp_offset,tmp_distance; + unsigned int tmp_id; + + segment_ids.clear(); + distances.clear(); + lateral_offsets.clear(); + for(unsigned int i=0;i<this->roads.size();i++) + { + if(this->roads[i]->get_closest_segment_id(point,tmp_id,tmp_distance,tmp_offset,angle_tol)) + { + if(fabs(tmp_offset)<offset_tol) + { + distances.push_back(tmp_distance); + lateral_offsets.push_back(tmp_offset); + segment_ids.push_back(tmp_id); + } + } + } + for(unsigned int i=0;i<this->junctions.size();i++) + { + if(this->junctions[i]->get_closest_segment_ids(point,tmp_ids,tmp_distances,tmp_offsets,angle_tol,offset_tol)) + { + for(unsigned int j=0;j<tmp_ids.size();j++) + { + distances.push_back(tmp_distances[j]); + lateral_offsets.push_back(tmp_offsets[j]); + segment_ids.push_back(tmp_ids[j]); + } + } + } + + if(segment_ids.size()>0) + return true; + else + return false; +} + +void CRoadMap::get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw) +{ + std::vector<double> partial_x,partial_y,partial_heading; + + x.clear(); + y.clear(); + yaw.clear(); + for(unsigned int i=0;i<this->roads.size();i++) + { + this->roads[i]->get_segment_geometry(partial_x,partial_y,partial_heading); + x.insert(x.end(),partial_x.begin(),partial_x.end()); + y.insert(y.end(),partial_y.begin(),partial_y.end()); + yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); + } + for(unsigned int i=0;i<this->junctions.size();i++) + { + this->junctions[i]->get_segment_geometry(partial_x,partial_y,partial_heading); + x.insert(x.end(),partial_x.begin(),partial_x.end()); + y.insert(y.end(),partial_y.begin(),partial_y.end()); + yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); + } +} + +void CRoadMap::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw) +{ + std::vector<double> partial_x,partial_y,partial_heading; + + x.clear(); + y.clear(); + yaw.clear(); + for(unsigned int i=0;i<this->roads.size();i++) + { + this->roads[i]->get_lane_geometry(partial_x,partial_y,partial_heading); + x.insert(x.end(),partial_x.begin(),partial_x.end()); + y.insert(y.end(),partial_y.begin(),partial_y.end()); + yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); + } + for(unsigned int i=0;i<this->junctions.size();i++) + { + this->junctions[i]->get_lane_geometry(partial_x,partial_y,partial_heading); + x.insert(x.end(),partial_x.begin(),partial_x.end()); + y.insert(y.end(),partial_y.begin(),partial_y.end()); + yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); + } +} + +CRoadMap::~CRoadMap() +{ + this->free(); +} + diff --git a/src/road_segment.cpp b/src/road_segment.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3dca6c5f1c16f667809a29b0091f1e797d5d84fd --- /dev/null +++ b/src/road_segment.cpp @@ -0,0 +1,470 @@ +#include "road_segment.h" +#include "exceptions.h" +#include "common.h" + +CRoadSegment::CRoadSegment(unsigned int lanes_in,unsigned int lanes_out) +{ + this->id=-1; + this->resolution=0.1; + this->parent_road=NULL; + this->parent_junction=NULL; + this->prev_segments.clear(); + this->next_segments.clear(); + this->connectivity=Eigen::MatrixXd::Zero(lanes_in,lanes_out); + this->start_point.x=std::numeric_limits<double>::max(); +} + +unsigned int CRoadSegment::get_index_by_id(const std::vector<CRoadSegment *> &segments,unsigned int id) +{ + std::vector<CRoadSegment *>::const_iterator it; + unsigned int index; + + for(it=segments.begin(),index=0;it!=segments.end();it++,index++) + if((*it)->id==id) + return index; + + return -1; +} + +void CRoadSegment::set_id(unsigned int id) +{ + this->id=id; +} + +void CRoadSegment::set_parent_road(CRoad *road) +{ + if(road==NULL) + throw CException(_HERE_,"Invalid road reference"); + this->parent_road=road; +} + +void CRoadSegment::set_parent_junction(CJunction *junction) +{ + if(junction==NULL) + throw CException(_HERE_,"Invalid junction reference"); + this->parent_junction=junction; +} + +void CRoadSegment::add_prev_segment(CRoadSegment *segment) +{ + if(segment==NULL) + throw CException(_HERE_,"Invalid road segment reference"); + if(this->has_prev_segment(segment)) + throw CException(_HERE_,"Previous road segment already present"); + this->prev_segments.push_back(segment); +} + +bool CRoadSegment::has_prev_segment(CRoadSegment *segment) +{ + if(segment==NULL) + throw CException(_HERE_,"Invalid road segment reference"); + for(unsigned int i=0;i<this->prev_segments.size();i++) + if(this->prev_segments[i]==segment) + return true; + + return false; +} + +void CRoadSegment::remove_prev_segment(CRoadSegment *segment) +{ + std::vector<CRoadSegment *>::iterator it; + + if(segment==NULL) + throw CException(_HERE_,"Invalid road segment reference"); + if(!this->has_prev_segment(segment)) + throw CException(_HERE_,"Previous road segment not present"); + for(it=this->prev_segments.begin();it!=this->prev_segments.end();it++) + { + if((*it)==segment) + { + this->prev_segments.erase(it); + break; + } + } +} + +void CRoadSegment::add_next_segment(CRoadSegment *segment) +{ + if(segment==NULL) + throw CException(_HERE_,"Invalid road segment reference"); + if(this->has_next_segment(segment)) + throw CException(_HERE_,"Next road segment already present"); + this->next_segments.push_back(segment); +} + +bool CRoadSegment::has_next_segment(CRoadSegment *segment) +{ + if(segment==NULL) + throw CException(_HERE_,"Invalid road segment reference"); + for(unsigned int i=0;i<this->next_segments.size();i++) + if(this->next_segments[i]==segment) + return true; + + return false; +} + +void CRoadSegment::remove_next_segment(CRoadSegment *segment) +{ + std::vector<CRoadSegment *>::iterator it; + + if(segment==NULL) + throw CException(_HERE_,"Invalid road segment reference"); + if(!this->has_next_segment(segment)) + throw CException(_HERE_,"Next road segment not present"); + for(it=this->next_segments.begin();it!=this->next_segments.end();it++) + { + if((*it)==segment) + { + this->next_segments.erase(it); + break; + } + } +} + +CRoad &CRoadSegment::get_parent_road(void) +{ + if(this->parent_road==NULL) + throw CException(_HERE_,"Inavalid parent road reference"); + return *this->parent_road; +} + +unsigned int CRoadSegment::get_id(void) +{ + return this->id; +} + +void CRoadSegment::set_resolution(double resolution) +{ + this->resolution=resolution; + if(this->start_point.x!=std::numeric_limits<double>::max()) + this->spline.generate(this->resolution,(unsigned int)10); +} + +double CRoadSegment::get_resolution(void) +{ + return this->resolution; +} + +bool CRoadSegment::has_parent_road(void) +{ + if(this->parent_road==NULL) + return false; + else + return true; +} + +CJunction &CRoadSegment::get_parent_junction(void) +{ + if(this->parent_junction==NULL) + throw CException(_HERE_,"Inavalid parent junction reference"); + return *this->parent_junction; +} + +bool CRoadSegment::has_parent_junction(void) +{ + if(this->parent_junction==NULL) + return false; + else + return true; +} + +unsigned int CRoadSegment::get_num_prev_segments(void) +{ + return this->prev_segments.size(); +} + +CRoadSegment &CRoadSegment::get_prev_segment_by_index(unsigned int index) +{ + if(index<0 || index >= this->prev_segments.size()) + throw CException(_HERE_,"Invalid previous segment index"); + return *this->prev_segments[index]; +} + +unsigned int CRoadSegment::get_num_next_segments(void) +{ + return this->next_segments.size(); +} + +CRoadSegment &CRoadSegment::get_next_segment_by_index(unsigned int index) +{ + if(index<0 || index >= this->next_segments.size()) + throw CException(_HERE_,"Invalid next segment index"); + return *this->next_segments[index]; +} + +unsigned int CRoadSegment::get_num_lanes_in(void) +{ + if(this->parent_road!=NULL) + return this->parent_road->get_num_lanes(); + else + return this->connectivity.rows(); +} + +unsigned int CRoadSegment::get_num_lanes_out(void) +{ + if(this->parent_road!=NULL) + return this->parent_road->get_num_lanes(); + else + return this->connectivity.cols(); +} + +double CRoadSegment::get_lane_width(void) +{ + double width; + + if(this->parent_road!=NULL) + return this->parent_road->get_lane_width(); + else + { + if(this->get_num_prev_segments()==1) + { + CRoadSegment &prev_segment=this->get_prev_segment_by_index(0); + if(prev_segment.has_parent_road()) + width=prev_segment.get_parent_road().get_lane_width(); + else + width=4.0; + } + else if(this->get_num_next_segments()==1) + { + CRoadSegment &next_segment=this->get_next_segment_by_index(0); + if(next_segment.has_parent_road()) + width=next_segment.get_parent_road().get_lane_width(); + else + width=4.0; + } + else + width=0.0; + } + + return width; +} + +double CRoadSegment::get_lane_speed(void) +{ + double speed; + + if(this->parent_road!=NULL) + return this->parent_road->get_lane_width(); + else + { + if(this->get_num_prev_segments()==1) + { + CRoadSegment &prev_segment=this->get_prev_segment_by_index(0); + if(prev_segment.has_parent_road()) + speed=prev_segment.get_parent_road().get_lane_speed(); + else + speed=50.0; + } + else if(this->get_num_next_segments()==1) + { + CRoadSegment &next_segment=this->get_next_segment_by_index(0); + if(next_segment.has_parent_road()) + speed=next_segment.get_parent_road().get_lane_speed(); + else + speed=50.0; + } + else + speed=0.0; + } + + return speed; +} + +unsigned int CRoadSegment::get_lane(double lateral_offset) +{ + unsigned int num_lanes; + double lane_width; + + if(lateral_offset>0.0) + return 0; + num_lanes=std::max(this->get_num_lanes_in(),this->get_num_lanes_out()); + lane_width=this->get_lane_width(); + for(unsigned int i=0;i<num_lanes;i++) + { + if(fabs(lateral_offset)<lane_width) + return i; + else + lateral_offset+=lane_width; + } + return num_lanes-1; +} + +void CRoadSegment::set_geometry(TPoint &start_point,TPoint &end_point) +{ + this->start_point=start_point; + this->end_point=end_point; + this->spline.set_start_point(start_point); + this->spline.set_end_point(end_point); + this->spline.generate(this->resolution,(unsigned int)10); +} + +void CRoadSegment::get_start_point(TPoint &point) +{ + point=this->start_point; +} + +void CRoadSegment::get_end_point(TPoint &point) +{ + point=this->end_point; +} + +void CRoadSegment::set_full_connectivity(void) +{ + for(unsigned int i=0;i<this->connectivity.rows();i++) + for(unsigned int j=0;j<this->connectivity.cols();j++) + this->connectivity(i,j)=1.0; +} + +void CRoadSegment::link_lanes(unsigned int lane1,unsigned int lane2) +{ + if(lane1>=this->connectivity.rows()) + throw CException(_HERE_,"Invalid lane1 index"); + if(lane2>=this->connectivity.cols()) + throw CException(_HERE_,"Invalid lane2 index"); + this->connectivity(lane1,lane2)=1.0; +} + +void CRoadSegment::unlink_lanes(unsigned int lane1,unsigned int lane2) +{ + if(lane1>=this->connectivity.rows()) + throw CException(_HERE_,"Invalid lane1 index"); + if(lane2>=this->connectivity.cols()) + throw CException(_HERE_,"Invalid lane2 index"); + this->connectivity(lane1,lane2)=0.0; +} + +bool CRoadSegment::are_lanes_linked(unsigned int lane1,unsigned int lane2) +{ + if(lane1>=this->connectivity.rows()) + throw CException(_HERE_,"Invalid lane1 index"); + if(lane2>=this->connectivity.cols()) + throw CException(_HERE_,"Invalid lane2 index"); + if(this->connectivity(lane1,lane2)==1.0) + return true; + else + return false; +} + +void CRoadSegment::get_connectivity_matrix(Eigen::MatrixXd &connectivity) +{ + connectivity=this->connectivity; +} + +bool CRoadSegment::is_input_lane_connected(unsigned int lane) +{ + unsigned int num=0; + + if(lane>=this->connectivity.rows()) + throw CException(_HERE_,"Invalid lane index"); + for(unsigned int i=0;i<this->connectivity.cols();i++) + if(this->connectivity(lane,i)==1.0) + num++; + if(num>0) + return true; + else + return false; +} + +double CRoadSegment::get_length(void) +{ + return this->spline.get_length(); +} + +bool CRoadSegment::get_point_at(double distance, double lateral_offset,TPoint &point) +{ + TPoint segment_point; + + if(this->spline.evaluate(distance,segment_point)) + { + point.x=segment_point.x-lateral_offset*sin(segment_point.heading); + point.y=segment_point.y+lateral_offset*cos(segment_point.heading); + point.heading=normalize_angle(segment_point.heading); + if(segment_point.curvature>0.0) + point.curvature=1.0/((1.0/segment_point.curvature)-lateral_offset); + else + point.curvature=1.0/((1.0/segment_point.curvature)+lateral_offset); + + return true; + } + else + return false; +} + +bool CRoadSegment::get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol) +{ + double cross; + + distance=this->spline.find_closest_point(target_point.x,target_point.y,closest_point); + if(distance==std::numeric_limits<double>::max()) + return false; + else + { + if(fabs(diff_angle(target_point.heading,closest_point.heading))<angle_tol) + { + lateral_offset=sqrt(pow(target_point.x-closest_point.x,2.0)+pow(target_point.y-closest_point.y,2.0)); + /* check the side */ + cross=(this->end_point.x-this->start_point.x)*(target_point.y-this->start_point.y)-(this->end_point.y-this->start_point.y)*(target_point.x-this->start_point.x); + if(cross<0.0) + lateral_offset*=-1.0; + return true; + } + else + return false; + } +} + +void CRoadSegment::get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double lateral_offset,double start_length, double end_length) +{ + std::vector<double> curvature; + CG2Spline *new_spline=NULL,*partial_spline; + TPoint start_point,end_point; + + if(lateral_offset!=0.0) + { + if(!get_point_at(0.0,lateral_offset,start_point)) + throw CException(_HERE_,"Impossible to compute the new start point"); + if(!get_point_at(this->get_length(),lateral_offset,end_point)) + throw CException(_HERE_,"Impossible to compute the new start point"); + new_spline=new CG2Spline(start_point,end_point); + if(start_length!=0.0 || end_length!=std::numeric_limits<double>::max())// get partial spline + { + partial_spline=new CG2Spline; + if(!new_spline->get_part(partial_spline,start_length,end_length)) + { + delete new_spline; + delete partial_spline; + throw CException(_HERE_,"Impossible to generate partial spline"); + } + partial_spline->evaluate_all(x,y,curvature,yaw); + delete partial_spline; + } + else + new_spline->evaluate_all(x,y,curvature,yaw); + delete new_spline; + } + else + { + if(start_length!=0.0 || end_length!=std::numeric_limits<double>::max())// get partial spline + { + partial_spline=new CG2Spline; + if(!this->spline.get_part(partial_spline,start_length,end_length)) + { + delete partial_spline; + throw CException(_HERE_,"Impossible to generate partial spline"); + } + partial_spline->evaluate_all(x,y,curvature,yaw); + delete partial_spline; + } + else + this->spline.evaluate_all(x,y,curvature,yaw); + } +} + +CRoadSegment::~CRoadSegment() +{ + this->parent_road=NULL; + this->parent_junction=NULL; + this->prev_segments.clear(); + this->next_segments.clear(); +} + diff --git a/src/vel_profile.cpp b/src/vel_profile.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4b1be562573a07357c6513dee394f623c253c49e --- /dev/null +++ b/src/vel_profile.cpp @@ -0,0 +1,131 @@ +#include "vel_profile.h" +#include "exceptions.h" + +CVelProfile::CVelProfile() +{ + this->start_vel=0.0; + this->end_vel=0.0; + this->start_acc=0.0; + this->end_acc=0.0; + this->time=0.0; + this->length=0.0; + this->max_acc=0.0; + this->generated=false; +} + +CVelProfile::CVelProfile(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc) +{ + this->set_start_vel(start_vel); + this->set_end_vel(end_vel); + this->set_max_vel(max_vel); + this->set_start_acc(start_acc); + this->set_end_acc(end_acc); + this->set_max_acc(max_acc); +} + +void CVelProfile::set_start_vel(double vel) +{ + if(vel<0.0) + throw CException(_HERE_,"Invalid velocity value"); + else + this->start_vel=vel; +} + +double CVelProfile::get_start_vel(void) +{ + return this->start_vel; +} + +void CVelProfile::set_end_vel(double vel) +{ + if(vel<0.0) + throw CException(_HERE_,"Invalid velocity value"); + else + this->end_vel=vel; +} + +double CVelProfile::get_end_vel(void) +{ + return this->end_vel; +} + +void CVelProfile::set_max_vel(double vel) +{ + if(vel<0.0) + throw CException(_HERE_,"Invalid velocity value"); + else + this->max_vel=vel; +} + +double CVelProfile::get_max_vel(void) +{ + return this->max_vel; +} + +void CVelProfile::set_start_acc(double acc) +{ +// if(acc<0.0) +// throw CException(_HERE_,"Invalid acceleration value"); +// else + this->start_acc=acc; +} + +double CVelProfile::get_start_acc(void) +{ + return this->start_acc; +} + +void CVelProfile::set_end_acc(double acc) +{ +// if(acc<0.0) +// throw CException(_HERE_,"Invalid acceleration value"); +// else + this->end_acc=acc; +} + +double CVelProfile::get_end_acc(void) +{ + return this->end_acc; +} + +void CVelProfile::set_max_acc(double acc) +{ + if(acc<0.0) + throw CException(_HERE_,"Invalid acceleration value"); + else + this->max_acc=acc; +} + +double CVelProfile::get_max_acc(void) +{ + return this->max_acc; +} + +double CVelProfile::get_time(void) +{ + return this->time; +} + +double CVelProfile::get_length(void) +{ + return this->length; +} + +std::ostream& operator<< (std::ostream& out, const CVelProfile &profile) +{ + out << "Velocity profile:" << std::endl; + out << " Start velocity: " << profile.start_vel << " m/s, start acceleration: " << profile.start_acc << " m/s2" << std::endl; + out << " End velocity: " << profile.end_vel << "m/s, end acceleration: " << profile.end_acc << " m/s2" << std::endl; + out << " Maximum velocity: " << profile.max_vel << " m/s" << std::endl; + out << " Maximum acceleration: " << profile.max_acc << " m/s2" << std::endl; + out << " Length: " << profile.length << " m" << std::endl; + out << " Time: " << profile.time << " s" << std::endl; + + return out; +} + +CVelProfile::~CVelProfile() +{ + +} + diff --git a/src/vel_spline.cpp b/src/vel_spline.cpp new file mode 100644 index 0000000000000000000000000000000000000000..76d833c78afa5a2a8b5cb62b7ba8b544aab78531 --- /dev/null +++ b/src/vel_spline.cpp @@ -0,0 +1,387 @@ +#include "vel_spline.h" +#include "exceptions.h" +#include <math.h> +#include <boost/bind.hpp> + +CVelSpline::CVelSpline() +{ + this->coeff[0]=0.0; + this->coeff[1]=0.0; + this->coeff[2]=0.0; + this->coeff[3]=0.0; + // initialize optimization objects + this->time_grad.set_function(boost::bind(&CVelSpline::length_pow2,this,_1)); + this->time_grad.set_function_der(boost::bind(&CVelSpline::length_pow2_der,this,_1)); + this->time_grad.set_max_coord_inc(1.0); +} + +CVelSpline::CVelSpline(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc) : CVelProfile(start_vel,end_vel,max_vel,start_acc,end_acc,max_acc) +{ + this->coeff[0]=0.0; + this->coeff[1]=0.0; + this->coeff[2]=0.0; + this->coeff[3]=0.0; + // initialize optimization objects + this->time_grad.set_function(boost::bind(&CVelSpline::length_pow2,this,_1)); + this->time_grad.set_function_der(boost::bind(&CVelSpline::length_pow2_der,this,_1)); +} + +double CVelSpline::length_pow2(double time) +{ + double pow_t,length; + + length=-this->target_length; + pow_t=time; + length+=this->coeff[3]*pow_t; + pow_t*=time; + length+=(this->coeff[2]*pow_t)/2.0; + pow_t*=time; + length+=(this->coeff[1]*pow_t)/3.0; + pow_t*=time; + length+=(this->coeff[0]*pow_t)/4.0; + + return pow(length,2.0); +} + +double CVelSpline::length_pow2_der(double time) +{ + double pow_t,grad,func; + + func=-this->target_length; + grad=this->coeff[3]; + pow_t=time; + func+=this->coeff[3]*pow_t; + grad+=this->coeff[2]*pow_t; + pow_t*=time; + func+=(this->coeff[2]*pow_t)/2.0; + grad+=this->coeff[1]*pow_t; + pow_t*=time; + func+=(this->coeff[1]*pow_t)/3.0; + grad+=this->coeff[0]*pow_t; + pow_t*=time; + func+=(this->coeff[0]*pow_t)/4.0; + + return 2.0*grad*func; +} + +void CVelSpline::set_target_length(double length) +{ + if(length<0.0) + throw CException(_HERE_,"Invalid length value"); + else + this->target_length=length; +} + +void CVelSpline::compute_max_vel(void) +{ + double time1,time2,vel1,vel2; + + if(4.0*pow(this->coeff[1],2.0)>12*this->coeff[1]*this->coeff[2]) + { + time1=(-2.0*this->coeff[1]+sqrt(4.0*pow(this->coeff[1],2.0)-12*this->coeff[1]*this->coeff[2]))/(6.0*this->coeff[0]); + vel1=this->coeff[3]+this->coeff[2]*time1+this->coeff[1]*pow(time1,2.0)+this->coeff[0]*pow(time1,3.0); + time2=(-2.0*this->coeff[1]-sqrt(4.0*pow(this->coeff[1],2.0)-12*this->coeff[1]*this->coeff[2]))/(6.0*this->coeff[0]); + vel2=this->coeff[3]+this->coeff[2]*time2+this->coeff[1]*pow(time2,2.0)+this->coeff[0]*pow(time2,3.0); + if(time1>=0.0 && time1<=this->time) + { + if(time2>=0.0 && time2<=this->time) + this->max_vel=(vel1+vel2)/2.0; + else + this->max_vel=vel1; + } + else + { + if(time2>=0.0 && time2<=this->time) + this->max_vel=vel2; + else + this->max_vel=this->start_vel; + } + } + else + this->max_vel=0.0; +} + +void CVelSpline::generate_max_acc(double max_acc) +{ + double t1,t2,t3,T; + + this->coeff[2]=this->start_acc; + this->coeff[3]=this->start_vel; + if(this->start_vel==this->end_vel)// acceleration 0 + { + if(this->start_acc!=0.0 || this->start_acc!=this->end_acc) + { + /* no solution */ + throw CException(_HERE_,"Impossible to create spline"); + } + else + { + this->coeff[0]=0.0; + this->coeff[1]=0.0; + T=0.0; + } + } + else if(this->start_vel>this->end_vel)//deceleration + { + t1=sqrt((max_acc+this->end_acc)/(max_acc+this->start_acc)); + t2=max_acc+this->start_acc; + t3=pow(t2,2.0); + if(this->start_acc==-max_acc) + { + this->coeff[1]=0.0; + this->coeff[0]=-((max_acc-this->end_acc)*pow(2.0*max_acc+this->end_acc,2.0))/(27.0*pow(this->end_vel-this->start_vel,2.0)); + T=sqrt(3.0*(this->end_acc+max_acc))/(3.0*sqrt(this->coeff[0])); + } + else + { + this->coeff[1]=-(-pow(t1+1,2.0)*t3+pow(t1+1,3.0)*t3/3.0+this->start_acc*(t1+1)*t2)/(this->end_vel-this->start_vel); + this->coeff[0]=pow(this->coeff[1],2.0)/(3.0*t2); + T=(t1+1)*t2/this->coeff[1]; + } + } + else// acceleration + { + t1=sqrt((max_acc-this->end_acc)/(max_acc-this->start_acc)); + t2=max_acc-this->start_acc; + t3=pow(t2,2.0); + if(this->start_acc==max_acc) + { + this->coeff[1]=0.0; + this->coeff[0]=-((max_acc-this->end_acc)*pow(2.0*max_acc+this->end_acc,2.0))/(27.0*pow(this->end_vel-this->start_vel,2.0)); + T=-sqrt(3.0*(this->end_acc-max_acc))/(3.0*sqrt(this->coeff[0])); + } + else + { + this->coeff[1]=(pow(t1+1,2.0)*t3-pow(t1+1,3.0)*t3/3.0+this->start_acc*(t1+1)*t2)/(this->end_vel-this->start_vel); + this->coeff[0]=-pow(this->coeff[1],2.0)/(3.0*t2); + T=(t1+1)*t2/this->coeff[1]; + } + } + this->time=T; + this->length=this->start_vel*T; + T*=this->time; + this->length+=(this->start_acc/2.0)*T; + T*=this->time; + this->length+=(this->coeff[1]/3.0)*T; + T*=this->time; + this->length+=(this->coeff[0]/4.0)*T; + this->max_acc=max_acc; + this->compute_max_vel(); + this->generated=true; + this->time_grad.set_coord_constraints(this->time,0.0); +} + +bool CVelSpline::generate_max_length(double &max_len) +{ + double t,b,a; + bool status=true; + + this->coeff[2]=this->start_acc; + this->coeff[3]=this->start_vel; + if(fabs(this->start_acc-this->end_acc)<0.001) + { + if(fabs(this->end_vel-this->start_vel)<0.001) + { + this->generate_constant_vel(this->start_vel,max_len); + return true; + } + else + { + this->time=(2.0*max_len)/(this->end_vel+this->start_vel); + this->coeff[1]=(3.0*(this->end_vel-this->start_vel))/pow(this->time,2.0); + this->coeff[0]=-(2.0*this->coeff[1])/(3.0*this->time); + this->max_acc=this->start_acc-pow(this->coeff[1],2.0)/(3.0*this->coeff[0]); + } + } + else + { + if((this->end_acc-this->start_acc)<((3.0*pow(this->start_vel+this->end_vel,2.0))/(4.0*max_len))) + { + t=(3.0*this->end_vel+3.0*this->start_vel-sqrt(9.0*pow(this->end_vel,2.0)+18.0*this->end_vel*this->start_vel+9.0*pow(this->start_vel,2.0)-12.0*max_len*this->end_acc+12.0*max_len*this->start_acc))/(this->end_acc-this->start_acc); + b=-(3.0*(this->start_vel-this->end_vel)+(this->end_acc+2.0*this->start_acc)*t)/pow(t,2.0); + a=-(this->start_acc-this->end_acc+2.0*b*t)/(3.0*pow(t,2.0)); + } + else + { + t=(3.0*this->end_vel+3.0*this->start_vel)/(this->end_acc-this->start_acc); + b=-(3.0*(this->start_vel-this->end_vel)+(this->end_acc+2.0*this->start_acc)*t)/pow(t,2.0); + a=-(this->start_acc-this->end_acc+2.0*b*t)/(3.0*pow(t,2.0)); + max_len=(3.0*pow(this->start_vel+this->end_vel,2.0))/(4.0*(this->end_acc-this->start_acc)); + status=false; + } + this->max_acc=this->start_acc-pow(b,2.0)/(3.0*a); + this->time=t; + this->coeff[1]=b; + this->coeff[0]=a; + } + this->length=max_len; + this->compute_max_vel(); + this->generated=true; + this->time_grad.set_coord_constraints(this->time,0.0); + + return status; +} + +void CVelSpline::generate_max_acc(double max_acc,double start_vel,double start_acc,double end_vel,double end_acc) +{ + this->set_start_vel(start_vel); + this->set_start_acc(start_acc); + this->set_end_vel(end_vel); + this->set_end_acc(end_acc); + this->generate_max_acc(max_acc); +} + +bool CVelSpline::generate_max_length(double &max_len,double start_vel,double start_acc,double end_vel,double end_acc) +{ + this->set_start_vel(start_vel); + this->set_start_acc(start_acc); + this->set_end_vel(end_vel); + this->set_end_acc(end_acc); + return this->generate_max_length(max_len); +} + +void CVelSpline::generate_constant_vel(double vel, double length) +{ + this->coeff[0]=0.0; + this->coeff[1]=0.0; + this->coeff[2]=0.0; + this->coeff[3]=vel; + this->start_vel=vel; + this->end_vel=vel; + this->start_acc=0.0; + this->end_acc=0.0; + this->length=length; + if(vel==0.0) + this->time=1.0; + else + this->time=length/vel; + this->max_acc=0.0; + this->max_vel=vel; + this->generated=true; +} + +void CVelSpline::update_max_start_vel(double max_acc,double length,double max_end_vel) +{ + double T,vel; + double t1; + + this->set_end_vel(max_end_vel); + if(this->start_vel>max_end_vel)// decelerate + { + t1=2.0*max_acc*sqrt((max_acc+this->end_acc-this->start_acc)/max_acc); + T=-(6.0*this->end_vel-6.0*sqrt(pow(this->end_vel,2.0)+2.0*length*max_acc/3.0-length*(this->end_acc+this->start_acc)+length*t1/3.0))/(-3.0*(this->end_acc+this->start_acc)+2*max_acc+t1); + vel=this->end_vel+(max_acc*T)/3.0-(this->end_acc*T)/3.0-(2.0*this->start_acc*T)/3.0+(T*t1)/6.0; + this->set_start_vel(vel); + this->coeff[1]=-(t1/2.0+max_acc)/T; + this->coeff[0]=pow(this->coeff[1],2.0)/(3.0*max_acc); + this->time=T; + } + else//accelerate + { +// T1=(6.0*this->end_vel+6.0*sqrt(pow(this->end_vel,2.0)-2.0*length*max_acc/3.0-length*(this->end_acc+this->start_acc)-length*t1/3.0))/(3.0*(this->end_acc+this->start_acc)+2*max_acc+t1); +// T2=(6.0*this->end_vel-6.0*sqrt(pow(this->end_vel,2.0)-2.0*length*max_acc/3.0-length*(this->end_acc+this->start_acc)-length*t1/3.0))/(3.0*(this->end_acc+this->start_acc)+2*max_acc+t1); +// T3=(6.0*this->end_vel+6.0*sqrt(pow(this->end_vel,2.0)-2.0*length*max_acc/3.0-length*(this->end_acc+this->start_acc)+length*t1/3.0))/(3.0*(this->end_acc+this->start_acc)+2*max_acc-t1); +// T4=(6.0*this->end_vel-6.0*sqrt(pow(this->end_vel,2.0)-2.0*length*max_acc/3.0-length*(this->end_acc+this->start_acc)+length*t1/3.0))/(3.0*(this->end_acc+this->start_acc)+2*max_acc-t1); +// vel=this->end_vel+(max_acc*T4)/3.0-(this->end_acc*T4)/3.0-(2.0*this->start_acc*T4)/3.0+(max_acc*T4*t1)/3.0; +// this->set_start_vel(vel); +// this->coeff[1]=-(max_acc*(t1+1.0))/T4; +// this->coeff[0]=-pow(this->coeff[1],2.0)/(3.0*max_acc); +// this->time=T4; + } + this->max_acc=max_acc; + this->length=length; +} + +void CVelSpline::evaluate_at_time(double time,double &vel,double &acc,double &len,double start_len) +{ + double pow_t; + + if(!this->generated) + this->generate_max_acc(DEFAULT_MAX_ACC); + if(time>this->time) + throw CException(_HERE_,"Spline not defined at the desired time"); + else + { + vel=this->coeff[3]; + acc=this->coeff[2]; + len=start_len; + pow_t=time; + len+=this->coeff[3]*pow_t; + vel+=this->coeff[2]*pow_t; + acc+=2.0*this->coeff[1]*pow_t; + pow_t*=time; + len+=this->coeff[2]*pow_t/2.0; + vel+=this->coeff[1]*pow_t; + acc+=3.0*this->coeff[0]*pow_t; + pow_t*=time; + len+=this->coeff[1]*pow_t/3.0; + vel+=this->coeff[0]*pow_t; + pow_t*=time; + len+=this->coeff[0]*pow_t/4.0; + } +} + +void CVelSpline::evaluate_at_length(double length,double &vel,double &acc,double &time) +{ + double start_point,pow_t; + bool beyond_limits; + + start_point=this->time/2.0; + this->set_target_length(length); + if(!this->time_grad.compute(0.1,100,start_point,beyond_limits,false)) + throw CException(_HERE_,"No solution found for the spline"); + else + { + time=this->time_grad.get_coordinate(); + vel=this->coeff[3]; + acc=this->coeff[2]; + pow_t=time; + vel+=this->coeff[2]*pow_t; + acc+=2.0*this->coeff[1]*pow_t; + pow_t*=time; + vel+=this->coeff[1]*pow_t; + acc+=3.0*this->coeff[0]*pow_t; + pow_t*=time; + vel+=this->coeff[0]*pow_t; + } +} + +void CVelSpline::evaluate_all(double resolution,std::vector<double> &vel, std::vector<double> &acc,std::vector<double> &len,double start_len) +{ + unsigned int i=0,num_points; + double pow_t,t; + + if(resolution<0.0) + throw CException(_HERE_,"Invalid time resolution"); + if(!this->generated) + this->generate_max_acc(DEFAULT_MAX_ACC); + num_points=ceil(this->time/resolution); + vel.resize(num_points); + acc.resize(num_points); + len.resize(num_points); + for(i=0,t=0;i<num_points;i++,t+=this->time/(num_points-1)) + { + vel[i]=this->coeff[3]; + acc[i]=this->coeff[2]; + len[i]=start_len; + pow_t=t; + len[i]+=this->coeff[3]*pow_t; + vel[i]+=this->coeff[2]*pow_t; + acc[i]+=2.0*this->coeff[1]*pow_t; + pow_t*=t; + len[i]+=this->coeff[2]*pow_t/2.0; + vel[i]+=this->coeff[1]*pow_t; + acc[i]+=3.0*this->coeff[0]*pow_t; + pow_t*=t; + len[i]+=this->coeff[1]*pow_t/3.0; + vel[i]+=this->coeff[0]*pow_t; + pow_t*=t; + len[i]+=this->coeff[0]*pow_t/4.0; + } +} + +CVelSpline::~CVelSpline() +{ + +} + diff --git a/src/vel_trapezoid.cpp b/src/vel_trapezoid.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ebb3d76e81f69588b98d6344150b49005d687096 --- /dev/null +++ b/src/vel_trapezoid.cpp @@ -0,0 +1,136 @@ +#include "vel_trapezoid.h" +#include "exceptions.h" +#include <math.h> + +CVelTrapezoid::CVelTrapezoid() +{ + this->acc_time=0.0; + this->dec_time=0.0; +} + +CVelTrapezoid::CVelTrapezoid(double start_vel,double end_vel,double max_vel,double start_acc,double end_acc,double max_acc) : CVelProfile(start_vel,end_vel,max_vel,start_acc,end_acc,max_acc) +{ + this->acc_time=0.0; + this->dec_time=0.0; +} + +void CVelTrapezoid::generate(double start_vel,double end_vel,double max_vel, double max_acc,double length) +{ + this->set_start_vel(start_vel); + this->set_end_vel(end_vel); + this->set_max_vel(max_vel); + this->set_max_acc(max_acc); + if(start_vel==max_vel) + this->acc_time=0.0; + else + this->acc_time=fabs(max_vel-start_vel)/max_acc; + if(end_vel==max_vel) + this->dec_time=0.0; + else + this->dec_time=fabs(end_vel-max_vel)/max_acc; + this->acc_length=std::min(start_vel,max_vel)*this->acc_time+fabs(max_vel-start_vel)*this->acc_time/2.0; + this->dec_length=std::min(end_vel,max_vel)*this->dec_time+fabs(end_vel-max_vel)*this->dec_time/2.0; + this->length=length; + this->generated=true; +} + +bool CVelTrapezoid::generate(double start_vel,double end_vel,double max_vel, double max_acc,double length,double &new_start_vel,double &new_max_vel) +{ + /* + if(start_vel>max_vel) + throw CException(_HERE_,"Start velocity must be smaller than the maximum velocity"); + if(end_vel>max_vel) + throw CException(_HERE_,"End velocity must be smaller than the maximum velocity"); + if(max_acc<=0.0) + throw CException(_HERE_,"Acceleration must be positive"); + */ + if(start_vel>max_vel) + start_vel=max_vel; + if(end_vel>max_vel) + end_vel=max_vel; + if(max_acc<=0.0) + max_acc=0.0; + this->generate(start_vel,end_vel,max_vel,max_acc,length); + if((length-this->acc_length-this->dec_length)>0.0) + { + this->time=(length-this->acc_length-this->dec_length)/this->max_vel+this->acc_time+this->dec_time; + new_start_vel=this->start_vel; + new_max_vel=this->max_vel; + return true; + } + else + { + new_max_vel=sqrt(length*this->max_acc+(pow(start_vel,2.0)+pow(end_vel,2.0))/2.0); + if(new_max_vel>=start_vel) + { + this->generate(this->start_vel,this->end_vel,new_max_vel,this->max_acc,this->length); + this->time=(length-this->acc_length-this->dec_length)/new_max_vel+this->acc_time+this->dec_time; + new_start_vel=this->start_vel; + return true; + } + else + { + new_max_vel=sqrt(pow(end_vel,2.0)+2*length*this->max_acc); + this->generate(this->start_vel,this->end_vel,new_max_vel,this->max_acc,this->length); + this->time=(length-this->acc_length-this->dec_length)/new_max_vel+this->acc_time+this->dec_time; + new_start_vel=this->start_vel; + return true; + } + } +} + +void CVelTrapezoid::evaluate_at_time(double time,double &vel,double &acc,double &len,double start_len) +{ + if(time>this->time) + throw CException(_HERE_,"Trapezoid not defined at the desired time"); + if(!this->generated) + throw CException(_HERE_,"Trapezoid not generated"); + if(time<=this->acc_time) + { + vel=this->max_acc*time+this->start_vel; + acc=this->max_acc; + len=start_len+this->max_acc*pow(time,2.0)/2.0+this->start_vel*time; + } + else if(time>=this->time-this->dec_time) + { + vel=this->max_vel-this->max_acc*(time-this->time+this->dec_time); + acc=-this->max_acc; + len=start_len+this->max_acc*pow(this->acc_time,2.0)/2.0+this->start_vel*this->acc_time; + len+=this->max_vel*(this->time-this->acc_time-this->dec_time); + len+=-this->max_acc*pow(time-this->time+this->dec_time,2.0)/2.0+this->max_vel*(time-this->time+this->dec_time); + } + else + { + vel=this->max_vel; + acc=0.0; + len=start_len+this->max_acc*pow(this->acc_time,2.0)/2.0+this->start_vel*this->acc_time; + len+=this->max_vel*(time-this->acc_time); + } +} + +void CVelTrapezoid::evaluate_at_length(double length,double &vel,double &acc,double &time) +{ +} + +void CVelTrapezoid::evaluate_all(double resolution,std::vector<double> &vel, std::vector<double> &acc,std::vector<double> &len,double start_len) +{ + unsigned int i=0,num_points; + double t; + + if(time>this->time) + throw CException(_HERE_,"Trapezoid not defined at the desired time"); + if(!this->generated) + throw CException(_HERE_,"Trapezoid not generated"); + num_points=ceil(this->time/resolution); + vel.resize(num_points); + acc.resize(num_points); + len.resize(num_points); + for(i=0,t=0;i<num_points;i++,t+=resolution) + this->evaluate_at_time(t,vel[i],acc[i],len[i],start_len); +} + +CVelTrapezoid::~CVelTrapezoid() +{ + +} + diff --git a/src/xml/add.xodr b/src/xml/add.xodr new file mode 100644 index 0000000000000000000000000000000000000000..3a1f52624f118d2b1351a9bd44df25b2806a688e --- /dev/null +++ b/src/xml/add.xodr @@ -0,0 +1,2498 @@ +<?xml version="1.0" ?> +<OpenDRIVE> + <header revMajor="1" revMinor="1" name="Testfile" version="1" date="Thu Feb 8 14:24:06 2007" north="2.0000000000000000e+03" south="-2.0000000000000000e+03" east="2.0000000000000000e+03" west="-2.0000000000000000e+03" /> + <road name="road0" length="1.3119999999999999e+01" id="0" junction="-1"> + <link> + <predecessor elementType="junction" elementId="1" contactPoint="end" /> + <successor elementType="road" elementId="1" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="8.3379999999999995e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.3119999999999999e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="1.0500000000000000e+01" t="1.4910000000000000e+01" id="2005" name="building5" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.4779999999999999e+01" width="9.8000000000000007e+00" height="2.0000000000000000e+00" /> + </objects> + <signals> + <signal s="1.0500000000000000e+01" t="7.5199999999999996e+00" id="101" name="locsign1" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="1.2800000000000001e+01" t="-1.6120000000000001e+01" id="102" name="locsign2" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="1.0500000000000000e+01" t="2.2300000000000001e+01" id="106" name="locsign6" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road1" length="2.0232056689000000e+01" id="1" junction="-1"> + <link> + <predecessor elementType="road" elementId="0" contactPoint="end" /> + <successor elementType="road" elementId="2" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="9.6500000000000000e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-04"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" /> + </geometry> + <geometry s="1.0000000000000000e-04" x="9.6500100000000003e+01" y="1.7205000000129399e+01" hdg="3.8819876000000010e-06" length="2.0231856689000001e+01"> + <arc curvature="7.7639752000000006e-02" /> + </geometry> + <geometry s="2.0231956689000000e+01" x="1.0938004992571371e+02" y="3.0085050042260775e+01" hdg="1.5708002178211014e+00" length="1.0000000000000000e-04"> + <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road2" length="4.1550000000000002e+00" id="2" junction="-1"> + <link> + <predecessor elementType="road" elementId="1" contactPoint="end" /> + <successor elementType="road" elementId="3" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0938000000000000e+02" y="3.0085000000000001e+01" hdg="1.5707000000000000e+00" length="4.1550000000000002e+00"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals> + <signal s="2.1250000000000000e+00" t="1.0480000000000000e+01" id="103" name="locsign3" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="2.1250000000000000e+00" t="2.0280000000000001e+01" id="107" name="locsign7" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="2.1250000000000000e+00" t="-1.1119999999999999e+01" id="104" name="locsign4" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road3" length="2.0232056689000000e+01" id="3" junction="-1"> + <link> + <predecessor elementType="road" elementId="2" contactPoint="end" /> + <successor elementType="road" elementId="25" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0938000000000000e+02" y="3.4240000000000002e+01" hdg="1.5707000000000000e+00" length="1.0000000000000000e-04"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" /> + </geometry> + <geometry s="1.0000000000000000e-04" x="1.0938000000950328e+02" y="3.4240099999999551e+01" hdg="1.5707038819876000e+00" length="2.0231856689000001e+01"> + <arc curvature="7.7639752000000006e-02" /> + </geometry> + <geometry s="2.0231956689000000e+01" x="9.6501190711420790e+01" y="4.7121290559894419e+01" hdg="3.1415002178211013e+00" length="1.0000000000000000e-04"> + <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road4" length="4.0526745231000007e+01" id="4" junction="-1"> + <link> + <predecessor elementType="road" elementId="25" contactPoint="end" /> + <successor elementType="road" elementId="26" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="9.6280000000000001e+01" y="4.7119999999999997e+01" hdg="3.1415899999999999e+00" length="1.0000000000000000e-04"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="-7.7519378999999999e-02" /> + </geometry> + <geometry s="1.0000000000000000e-04" x="9.6279899999999998e+01" y="4.7120000000394555e+01" hdg="3.1415861240310501e+00" length="4.0526545231000000e+01"> + <arc curvature="-7.7519378999999999e-02" /> + </geometry> + <geometry s="4.0526645231000003e+01" x="9.6280068020569701e+01" y="7.2920000281067445e+01" hdg="-6.4952914806681861e-06" length="1.0000000000000000e-04"> + <spiral curvStart="-7.7519378999999999e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="2.0231900000000000e+01" t="-3.7880000000000003e+01" id="3000" name="fence0" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="barrier" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="2.0000000000000000e+00" width="1.2200000000000000e+02" height="2.0000000000000000e+00" /> + </objects> + <signals> + <signal s="2.0231900000000000e+01" t="-3.6880000000000003e+01" id="105" name="locsign5" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="2.0231900000000000e+01" t="1.5960000000000001e+01" id="121" name="locsign21" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="2.0231900000000000e+01" t="2.9960000000000001e+01" id="122" name="locsign22" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road5" length="2.0232056689000000e+01" id="5" junction="-1"> + <link> + <predecessor elementType="road" elementId="26" contactPoint="end" /> + <successor elementType="road" elementId="6" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="9.6680000000000007e+01" y="7.2920000000000002e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-04"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" /> + </geometry> + <geometry s="1.0000000000000000e-04" x="9.6680100000000010e+01" y="7.2920000000129406e+01" hdg="3.8819876000000010e-06" length="2.0231856689000001e+01"> + <arc curvature="7.7639752000000006e-02" /> + </geometry> + <geometry s="2.0231956689000000e+01" x="1.0956004992571371e+02" y="8.5800050042260779e+01" hdg="1.5708002178211014e+00" length="1.0000000000000000e-04"> + <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road6" length="4.2800000000000002e+00" id="6" junction="-1"> + <link> + <predecessor elementType="road" elementId="5" contactPoint="end" /> + <successor elementType="road" elementId="7" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0956000000000000e+02" y="8.5799999999999997e+01" hdg="1.5707000000000000e+00" length="4.2800000000000002e+00"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals> + <signal s="2.1250000000000000e+00" t="-1.1119999999999999e+01" id="108" name="locsign8" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="2.1250000000000000e+00" t="1.1880000000000001e+01" id="109" name="locsign9" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="2.1250000000000000e+00" t="2.3879999999999999e+01" id="110" name="locsign10" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="2.1250000000000000e+00" t="4.1719999999999999e+01" id="119" name="locsign19" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="2.1250000000000000e+00" t="5.5719999999999999e+01" id="120" name="locsign20" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road7" length="2.0232056689000000e+01" id="7" junction="-1"> + <link> + <predecessor elementType="road" elementId="6" contactPoint="end" /> + <successor elementType="road" elementId="8" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0956000000000000e+02" y="9.0079999999999998e+01" hdg="1.5707000000000000e+00" length="1.0000000000000000e-04"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" /> + </geometry> + <geometry s="1.0000000000000000e-04" x="1.0956000000950328e+02" y="9.0080099999999547e+01" hdg="1.5707038819876000e+00" length="2.0231856689000001e+01"> + <arc curvature="7.7639752000000006e-02" /> + </geometry> + <geometry s="2.0231956689000000e+01" x="9.6681190711420797e+01" y="1.0296129055989442e+02" hdg="3.1415002178211013e+00" length="1.0000000000000000e-04"> + <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road8" length="1.3240000000000000e+01" id="8" junction="-1"> + <link> + <predecessor elementType="road" elementId="7" contactPoint="end" /> + <successor elementType="junction" elementId="2" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="9.6680000000000007e+01" y="1.0295999999999999e+02" hdg="3.1415899999999999e+00" length="1.3240000000000000e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="5.0000000000000000e+00" t="1.5000000000000000e+01" id="2007" name="building7" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" radius="6.0000000000000000e+00" height="2.0000000000000000e+00" /> + </objects> + <signals> + <signal s="5.0000000000000000e+00" t="9.0000000000000000e+00" id="111" name="locsign11" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="5.0000000000000000e+00" t="2.1000000000000000e+01" id="112" name="locsign12" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="5.0000000000000000e+00" t="-1.6120000000000001e+01" id="113" name="locsign13" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road9" length="1.5050000000000001e+01" id="9" junction="-1"> + <link> + <predecessor elementType="junction" elementId="2" contactPoint="end" /> + <successor elementType="junction" elementId="3" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="6.7640000000000001e+01" y="1.0295999999999999e+02" hdg="3.1415899999999999e+00" length="1.5050000000000001e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="7.2400000000000002e+00" t="1.5480000000000000e+01" id="2002" name="building2" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.4000000000000000e+01" width="1.4000000000000000e+01" height="2.0000000000000000e+00" /> + <object s="7.2400000000000002e+00" t="4.3659999999999997e+01" id="2003" name="building3" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.4000000000000000e+01" width="1.4000000000000000e+01" height="2.0000000000000000e+00" /> + <object s="7.4000000000000004e+00" t="-1.7120000000000001e+01" id="3003" name="fence3" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="2.0000000000000000e+00" width="1.2000000000000000e+02" height="2.0000000000000000e+00" /> + <object s="7.5250000000000004e+00" t="-6.5999999999999996e+00" id="4006" name="parking6" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00"> + <parkingSpace access="all" restrictions="none"> + <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" /> + </parkingSpace> + </object> + </objects> + <signals> + <signal s="7.2400000000000002e+00" t="-1.6120000000000001e+01" id="114" name="locsign14" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="7.2400000000000002e+00" t="8.4800000000000004e+00" id="115" name="locsign15" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="7.2400000000000002e+00" t="2.2480000000000000e+01" id="116" name="locsign16" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="7.2400000000000002e+00" t="3.6659999999999997e+01" id="117" name="locsign17" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="7.2400000000000002e+00" t="5.0659999999999997e+01" id="118" name="locsign18" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road10" length="1.3119999999999999e+01" id="10" junction="-1"> + <link> + <predecessor elementType="junction" elementId="3" contactPoint="end" /> + <successor elementType="road" elementId="11" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="3.6789999999999999e+01" y="1.0295999999999999e+02" hdg="3.1415899999999999e+00" length="1.3119999999999999e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals> + <signal s="8.3750000000000000e+00" t="-1.6120000000000001e+01" id="123" name="locsign23" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="8.3750000000000000e+00" t="8.4499999999999993e+00" id="124" name="locsign24" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="8.3750000000000000e+00" t="2.2230000000000000e+01" id="125" name="locsign25" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="7.8399999999999999e+00" t="3.5969999999999999e+01" id="132" name="locsign32" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="7.8399999999999999e+00" t="4.9969999999999999e+01" id="133" name="locsign33" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road11" length="2.0232056689000000e+01" id="11" junction="-1"> + <link> + <predecessor elementType="road" elementId="10" contactPoint="end" /> + <successor elementType="road" elementId="12" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="2.3859999999999999e+01" y="1.0295999999999999e+02" hdg="3.1415899999999999e+00" length="1.0000000000000000e-04"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" /> + </geometry> + <geometry s="1.0000000000000000e-04" x="2.3859900000000000e+01" y="1.0296000000013595e+02" hdg="3.1415938819875997e+00" length="2.0231856689000001e+01"> + <arc curvature="7.7639752000000006e-02" /> + </geometry> + <geometry s="2.0231956689000000e+01" x="1.0979915895962318e+01" y="9.0079984136153584e+01" hdg="4.7123902178211008e+00" length="1.0000000000000000e-04"> + <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road12" length="7.2100000000000000e+00" id="12" junction="-1"> + <link> + <predecessor elementType="road" elementId="11" contactPoint="end" /> + <successor elementType="junction" elementId="4" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="9.0079999999999998e+01" hdg="4.7123889800000001e+00" length="7.2100000000000000e+00"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="2.4600000000000000e+00" t="1.7504999999999999e+01" id="2000" name="building0" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.1570000000000000e+01" width="1.3779999999999999e+01" height="2.0000000000000000e+00" /> + </objects> + <signals> + <signal s="2.4600000000000000e+00" t="1.1720000000000001e+01" id="126" name="locsign26" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="2.4600000000000000e+00" t="2.3289999999999999e+01" id="127" name="locsign27" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="2.4600000000000000e+00" t="-1.1119999999999999e+01" id="128" name="locsign28" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road13" length="1.4170000000000000e+01" id="13" junction="-1"> + <link> + <predecessor elementType="junction" elementId="4" contactPoint="end" /> + <successor elementType="junction" elementId="5" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="6.7069999999999993e+01" hdg="4.7123889800000001e+00" length="1.4170000000000000e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="7.2000000000000002e+00" t="1.8039999999999999e+01" id="2001" name="building1" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.4000000000000000e+01" width="1.4000000000000000e+01" height="2.0000000000000000e+00" /> + <object s="7.2000000000000002e+00" t="-1.2119999999999999e+01" id="3001" name="fence1" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="2.0000000000000000e+00" width="1.2200000000000000e+02" height="2.0000000000000000e+00" /> + </objects> + <signals> + <signal s="7.2000000000000002e+00" t="-1.1119999999999999e+01" id="129" name="locsign29" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="7.2000000000000002e+00" t="1.1039999999999999e+01" id="130" name="locsign30" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="7.2000000000000002e+00" t="2.5039999999999999e+01" id="131" name="locsign31" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road14" length="7.0149999999999997e+00" id="14" junction="-1"> + <link> + <predecessor elementType="junction" elementId="5" contactPoint="end" /> + <successor elementType="road" elementId="15" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="3.7100000000000001e+01" hdg="4.7123889800000001e+00" length="7.0149999999999997e+00"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals> + <signal s="4.2999999999999998e+00" t="-1.1119999999999999e+01" id="134" name="locsign34" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="4.2999999999999998e+00" t="1.0840000000000000e+01" id="135" name="locsign35" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="4.2999999999999998e+00" t="2.4840000000000000e+01" id="136" name="locsign36" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="5.4299999999999997e+00" t="4.1430000000000000e+01" id="143" name="locsign43" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="5.4299999999999997e+00" t="5.5979999999999997e+01" id="144" name="locsign44" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road15" length="2.0232056689000000e+01" id="15" junction="-1"> + <link> + <predecessor elementType="road" elementId="14" contactPoint="end" /> + <successor elementType="road" elementId="16" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="3.0085000000000001e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-04"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="7.7639752000000006e-02" /> + </geometry> + <geometry s="1.0000000000000000e-04" x="1.0980000000129360e+01" y="3.0084900000000001e+01" hdg="4.7123928619875999e+00" length="2.0231856689000001e+01"> + <arc curvature="7.7639752000000006e-02" /> + </geometry> + <geometry s="2.0231956689000000e+01" x="2.3860050037305950e+01" y="1.7204950069331481e+01" hdg="6.2831891978211010e+00" length="1.0000000000000000e-04"> + <spiral curvStart="7.7639752000000006e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road16" length="1.2960000000000001e+01" id="16" junction="-1"> + <link> + <predecessor elementType="road" elementId="15" contactPoint="end" /> + <successor elementType="junction" elementId="0" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="2.3859999999999999e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.2960000000000001e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="4.9600000000000000e+00" t="1.5779999999999999e+01" id="2004" name="building4" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.0000000000000000e+01" width="1.4000000000000000e+01" height="2.0000000000000000e+00" /> + </objects> + <signals> + <signal s="4.9600000000000000e+00" t="1.0779999999999999e+01" id="137" name="locsign37" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="4.9600000000000000e+00" t="2.0780000000000001e+01" id="138" name="locsign38" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="4.9600000000000000e+00" t="-1.6120000000000001e+01" id="139" name="locsign39" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road17" length="1.4960000000000001e+01" id="17" junction="-1"> + <link> + <predecessor elementType="junction" elementId="0" contactPoint="end" /> + <successor elementType="junction" elementId="1" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="unknown" /> + <planView> + <geometry s="0.0000000000000000e+00" x="5.2619999999999997e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.4960000000000001e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="7.2249999999999996e+00" t="1.4650000000000000e+01" id="2006" name="building6" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="1.3080000000000000e+01" width="1.4550000000000001e+01" height="2.0000000000000000e+00" /> + <object s="7.4000000000000004e+00" t="-1.7120000000000001e+01" id="3002" name="fence2" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="building" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="2.0000000000000000e+00" width="1.2000000000000000e+02" height="2.0000000000000000e+00" /> + <object s="7.4800000000000004e+00" t="-6.5999999999999996e+00" id="4001" name="parking1" dynamic="no" orientation="none" zOffset="1.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00"> + <parkingSpace access="all" restrictions="none"> + <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" /> + </parkingSpace> + </object> + </objects> + <signals> + <signal s="7.2249999999999996e+00" t="-1.6120000000000001e+01" id="140" name="locsign40" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="7.2249999999999996e+00" t="8.1099999999999994e+00" id="141" name="locsign41" dynamic="no" orientation="+" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + <signal s="7.2249999999999996e+00" t="2.1190000000000001e+01" id="142" name="locsign42" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1000" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="road18" length="1.5740000000000000e+01" id="18" junction="-1"> + <link> + <predecessor elementType="junction" elementId="7" contactPoint="end" /> + <successor elementType="junction" elementId="5" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="3.6820000000000000e+01" y="4.2799999999999997e+01" hdg="3.1415899999999999e+00" length="1.5740000000000000e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road19" length="9.7949999999999999e+00" id="19" junction="-1"> + <link> + <predecessor elementType="junction" elementId="7" contactPoint="end" /> + <successor elementType="junction" elementId="0" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="4.6920000000000002e+01" y="3.7100000000000001e+01" hdg="4.7123889800000001e+00" length="9.7949999999999999e+00"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road20" length="3.9499313349999994e+01" id="20" junction="-1"> + <link> + <predecessor elementType="junction" elementId="7" contactPoint="end" /> + <successor elementType="junction" elementId="1" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="unknown" /> + <planView> + <geometry s="0.0000000000000000e+00" x="5.2619999999999997e+01" y="4.7200000000000003e+01" hdg="0.0000000000000000e+00" length="1.2359999999999999e+01"> + <line /> + </geometry> + <geometry s="1.2359999999999999e+01" x="6.4979999999999990e+01" y="4.7200000000000003e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-04"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="-7.8740157000000005e-02" /> + </geometry> + <geometry s="1.2360099999999999e+01" x="6.4980099999999993e+01" y="4.7199999999868773e+01" hdg="-3.9370078499816491e-06" length="1.9949113350000001e+01"> + <arc curvature="-7.8740157000000005e-02" /> + </geometry> + <geometry s="3.2309213350000000e+01" x="7.7680050077372044e+01" y="3.4499950044481970e+01" hdg="-1.5708002541976460e+00" length="1.0000000000000000e-04"> + <spiral curvStart="-7.8740157000000005e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + <geometry s="3.2309313350000004e+01" x="7.7680050076716839e+01" y="3.4499850044481967e+01" hdg="-1.5708041912054962e+00" length="7.1900000000000004e+00"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="3.4249099999999999e+01" t="4.4000000000000004e+00" id="4003" name="parking3" dynamic="no" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="1.5707000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00"> + <parkingSpace access="all" restrictions="none"> + <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="left" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" /> + </parkingSpace> + </object> + <object s="3.8949100000000001e+01" t="4.4000000000000004e+00" id="4004" name="parking4" dynamic="no" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="1.5707000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00"> + <parkingSpace access="all" restrictions="none"> + <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="left" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" /> + </parkingSpace> + </object> + </objects> + <signals /> + </road> + <road name="road21" length="1.4170000000000000e+01" id="21" junction="-1"> + <link> + <predecessor elementType="junction" elementId="6" contactPoint="end" /> + <successor elementType="junction" elementId="7" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="4.6920000000000002e+01" y="6.7069999999999993e+01" hdg="4.7123889800000001e+00" length="1.4170000000000000e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road22" length="1.5740000000000000e+01" id="22" junction="-1"> + <link> + <predecessor elementType="junction" elementId="4" contactPoint="end" /> + <successor elementType="junction" elementId="6" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="2.1079999999999998e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.5740000000000000e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road23" length="1.0015000000000001e+01" id="23" junction="-1"> + <link> + <predecessor elementType="junction" elementId="6" contactPoint="end" /> + <successor elementType="junction" elementId="3" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="4.2520000000000003e+01" y="8.2870000000000005e+01" hdg="1.5707000000000000e+00" length="1.0015000000000001e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road24" length="3.2847809632999997e+01" id="24" junction="-1"> + <link> + <predecessor elementType="junction" elementId="6" contactPoint="end" /> + <successor elementType="junction" elementId="2" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="5.2619999999999997e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.2420000000000000e+01"> + <line /> + </geometry> + <geometry s="1.2420000000000000e+01" x="6.5039999999999992e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-04"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="1.2048192700000000e-01" /> + </geometry> + <geometry s="1.2420100000000000e+01" x="6.5040099999999995e+01" y="7.7170000000200801e+01" hdg="6.0240963499719210e-06" length="1.3037609633000001e+01"> + <arc curvature="1.2048192700000000e-01" /> + </geometry> + <geometry s="2.5457709633000000e+01" x="7.3340050048819123e+01" y="8.5470050092700660e+01" hdg="1.5708023561539528e+00" length="1.0000000000000000e-04"> + <spiral curvStart="1.2048192700000000e-01" curvEnd="0.0000000000000000e+00" /> + </geometry> + <geometry s="2.5457809633000000e+01" x="7.3340050047814572e+01" y="8.5470150092700649e+01" hdg="1.5708083802503028e+00" length="7.3899999999999997e+00"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road25" length="2.0000000000000001e-01" id="25" junction="-1"> + <link> + <predecessor elementType="road" elementId="3" contactPoint="end" /> + <successor elementType="road" elementId="4" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="9.6480000000000004e+01" y="4.7119999999999997e+01" hdg="3.1415899999999999e+00" length="2.0000000000000001e-01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="road26" length="4.0000000000000002e-01" id="26" junction="-1"> + <link> + <predecessor elementType="road" elementId="4" contactPoint="end" /> + <successor elementType="road" elementId="5" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="9.6280000000000001e+01" y="7.2920000000000002e+01" hdg="0.0000000000000000e+00" length="4.0000000000000002e-01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <left> + <lane id="1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </left> + <center> + <lane id="0" type="driving" level="false"> + <link /> + <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="2.0000000000000001e-01" laneChange="both" /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_16_-1_17_-1" length="1.5799999999999997e+01" id="1000" junction="0"> + <link> + <predecessor elementType="road" elementId="16" contactPoint="end" /> + <successor elementType="road" elementId="17" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="3.6820000000000000e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.5799999999999997e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="1.4580000000000000e+01" t="-6.7500000000000000e+00" id="4000" name="parking0" dynamic="yes" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00"> + <parkingSpace access="all" restrictions="none"> + <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" /> + </parkingSpace> + </object> + </objects> + <signals /> + </road> + <road name="junction_17_1_16_1" length="1.5799999999999997e+01" id="1001" junction="0"> + <link> + <predecessor elementType="road" elementId="17" contactPoint="start" /> + <successor elementType="road" elementId="16" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="unknown" /> + <planView> + <geometry s="0.0000000000000000e+00" x="5.2619999999999997e+01" y="1.7204999999999998e+01" hdg="3.1415799999999998e+00" length="1.5799999999999997e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_19_-1_16_1" length="1.5867055652294562e+01" id="1002" junction="0"> + <link> + <predecessor elementType="road" elementId="19" contactPoint="end" /> + <successor elementType="road" elementId="16" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="4.6919999996231965e+01" y="2.7305000000000000e+01" hdg="4.7123889800000001e+00" length="1.2780810170376355e-04"> + <line /> + </geometry> + <geometry s="1.2780810170376355e-04" x="4.6919999996231915e+01" y="2.7304872191898298e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9011416593046367e-02" /> + </geometry> + <geometry s="1.1278081017037636e-03" x="4.6919999979729631e+01" y="2.7303872191898542e+01" hdg="4.7123394742917037e+00" length="1.5864927844192859e+01"> + <arc curvature="-9.9011416593046367e-02" /> + </geometry> + <geometry s="1.5866055652294563e+01" x="3.6819526797096088e+01" y="1.7204526819585467e+01" hdg="3.1415304942917039e+00" length="1.0000000000000000e-03"> + <spiral curvStart="-9.9011416593046367e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_19_-1_17_-1" length="1.3355597643755036e+01" id="1003" junction="0"> + <link> + <predecessor elementType="road" elementId="19" contactPoint="end" /> + <successor elementType="road" elementId="17" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="4.6919999996231965e+01" y="2.7305000000000000e+01" hdg="4.7123889800000001e+00" length="4.3999999923466024e+00"> + <line /> + </geometry> + <geometry s="4.3999999923466024e+00" x="4.6919999994539332e+01" y="2.2905000007653399e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543462205417634e-01" /> + </geometry> + <geometry s="4.4009999923466028e+00" x="4.6920000023778051e+01" y="2.2904000007654169e+01" hdg="4.7124766973110273e+00" length="8.9535976514084350e+00"> + <arc curvature="1.7543462205417634e-01" /> + </geometry> + <geometry s="1.3354597643755039e+01" x="5.2620484880042277e+01" y="1.7204370884176605e+01" hdg="6.2832477173110277e+00" length="1.0000000000000000e-03"> + <spiral curvStart="1.7543462205417634e-01" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_17_-1_0_-1" length="1.5799999999999997e+01" id="1004" junction="1"> + <link> + <predecessor elementType="road" elementId="17" contactPoint="end" /> + <successor elementType="road" elementId="0" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="unknown" /> + <planView> + <geometry s="0.0000000000000000e+00" x="6.7579999999999998e+01" y="1.7204999999999998e+01" hdg="0.0000000000000000e+00" length="1.5799999999999997e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="1.2200000000000000e+00" t="-6.7500000000000000e+00" id="4002" name="parking2" dynamic="no" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00"> + <parkingSpace access="all" restrictions="none"> + <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" /> + </parkingSpace> + </object> + </objects> + <signals> + <signal s="6.0000000000000000e+00" t="-9.0000000000000000e+00" id="10000" name="global_loc" dynamic="no" orientation="-" zOffset="0.0000000000000000e+00" country="DEU" type="1001" subtype="-1" value="0.0000000000000000e+00" /> + </signals> + </road> + <road name="junction_0_1_17_1" length="1.5799999999999997e+01" id="1005" junction="1"> + <link> + <predecessor elementType="road" elementId="0" contactPoint="start" /> + <successor elementType="road" elementId="17" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="8.3379999999999995e+01" y="1.7204999999999998e+01" hdg="3.1415799999999998e+00" length="1.5799999999999997e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_20_-1_17_1" length="1.5871901969384437e+01" id="1006" junction="1"> + <link> + <predecessor elementType="road" elementId="20" contactPoint="end" /> + <successor elementType="road" elementId="17" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="unknown" /> + <planView> + <geometry s="0.0000000000000000e+00" x="7.7679993531604623e+01" y="2.7309850044704316e+01" hdg="4.7123758087945031e+00" length="5.1174112856067211e-03"> + <line /> + </geometry> + <geometry s="5.1174112856067211e-03" x="7.7679993464200180e+01" y="2.7304732633419153e+01" hdg="4.7123758087945031e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9011480618728345e-02" /> + </geometry> + <geometry s="6.1174112856067211e-03" x="7.7679993434526679e+01" y="2.7303732633419703e+01" hdg="4.7123263030541942e+00" length="1.5864784558098831e+01"> + <arc curvature="-9.9011480618728345e-02" /> + </geometry> + <geometry s="1.5870901969384438e+01" x="6.7579526790493134e+01" y="1.7204526819215236e+01" hdg="3.1415304942596904e+00" length="1.0000000000000000e-03"> + <spiral curvStart="-9.9011480618728345e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_20_-1_0_-1" length="1.3360484496420572e+01" id="1007" junction="1"> + <link> + <predecessor elementType="road" elementId="20" contactPoint="end" /> + <successor elementType="road" elementId="0" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="unknown" /> + <planView> + <geometry s="0.0000000000000000e+00" x="7.7679993531604623e+01" y="2.7309850044704316e+01" hdg="4.7123758087945031e+00" length="4.4047104802418007e+00"> + <line /> + </geometry> + <geometry s="4.4047104802418007e+00" x="7.7679935514563283e+01" y="2.2905139564844603e+01" hdg="4.7123758087945031e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543263749645815e-01" /> + </geometry> + <geometry s="4.4057104802418010e+00" x="7.7679935530630473e+01" y="2.2904139564845075e+01" hdg="4.7124635251132512e+00" length="8.9537740161787713e+00"> + <arc curvature="1.7543263749645815e-01" /> + </geometry> + <geometry s="1.3359484496420572e+01" x="8.3380484873285440e+01" y="1.7204370881014821e+01" hdg="6.2832477163187477e+00" length="1.0000000000000000e-03"> + <spiral curvStart="1.7543263749645815e-01" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_8_-1_9_-1" length="1.5800000000085685e+01" id="1008" junction="2"> + <link> + <predecessor elementType="road" elementId="8" contactPoint="end" /> + <successor elementType="road" elementId="9" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="8.3440000000046624e+01" y="1.0296003513352886e+02" hdg="3.1415899999999999e+00" length="1.5800000000085685e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="1.4625000000000000e+01" t="-6.8499999999999996e+00" id="4005" name="parking5" dynamic="no" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00"> + <parkingSpace access="all" restrictions="none"> + <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" /> + </parkingSpace> + </object> + </objects> + <signals /> + </road> + <road name="junction_9_1_8_1" length="1.5800000000085685e+01" id="1009" junction="2"> + <link> + <predecessor elementType="road" elementId="9" contactPoint="start" /> + <successor elementType="road" elementId="8" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="6.7640000000000001e+01" y="1.0295999999999999e+02" hdg="6.2831700000000001e+00" length="1.5800000000085685e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_24_-1_8_1" length="1.5867172362152607e+01" id="1010" junction="2"> + <link> + <predecessor elementType="road" elementId="24" contactPoint="end" /> + <successor elementType="road" elementId="8" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="7.3339960972779124e+01" y="9.2860150092163821e+01" hdg="1.5708083802503028e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9009978376276273e-02" /> + </geometry> + <geometry s="1.0000000000000000e-03" x="7.3339960977227321e+01" y="9.2861150092163697e+01" hdg="1.5707588752611146e+00" length="1.5865051240397811e+01"> + <arc curvature="-9.9009978376276273e-02" /> + </geometry> + <geometry s="1.5866051240397811e+01" x="8.3440352076065167e+01" y="1.0296076392992921e+02" hdg="-3.9504989187477690e-05" length="1.0000000000000000e-03"> + <spiral curvStart="-9.9009978376276273e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + <geometry s="1.5867051240397810e+01" x="8.3441352076062429e+01" y="1.0296076385742090e+02" hdg="-8.9009978375560961e-05" length="1.2112175479650489e-04"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_24_-1_9_-1" length="1.3355306172018555e+01" id="1011" junction="2"> + <link> + <predecessor elementType="road" elementId="24" contactPoint="end" /> + <successor elementType="road" elementId="9" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="7.3339960972779124e+01" y="9.2860150092163821e+01" hdg="1.5708083802503028e+00" length="4.3999955486436164e+00"> + <line /> + </geometry> + <geometry s="4.3999955486436164e+00" x="7.3339907937628993e+01" y="9.7260145640487806e+01" hdg="1.5708083802503028e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7544143008383561e-01" /> + </geometry> + <geometry s="4.4009955486436168e+00" x="7.3339907896335291e+01" y="9.7261145640486603e+01" hdg="1.5708961009653448e+00" length="8.9533106233749393e+00"> + <arc curvature="1.7544143008383561e-01" /> + </geometry> + <geometry s="1.3354306172018557e+01" x="6.7639515111861243e+01" y="1.0296048485434106e+02" hdg="3.1416777207150419e+00" length="1.0000000000000000e-03"> + <spiral curvStart="1.7544143008383561e-01" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_12_-1_13_-1" length="1.5800000000000011e+01" id="1012" junction="4"> + <link> + <predecessor elementType="road" elementId="12" contactPoint="end" /> + <successor elementType="road" elementId="13" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0979999997226386e+01" y="8.2870000000000005e+01" hdg="4.7123889800000001e+00" length="1.5800000000000011e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_13_1_12_1" length="1.5800000000000011e+01" id="1013" junction="4"> + <link> + <predecessor elementType="road" elementId="13" contactPoint="start" /> + <successor elementType="road" elementId="12" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="6.7069999999999993e+01" hdg="1.5707889799999997e+00" length="1.5800000000000011e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_12_-1_22_-1" length="1.3355597644352770e+01" id="1014" junction="4"> + <link> + <predecessor elementType="road" elementId="12" contactPoint="end" /> + <successor elementType="road" elementId="22" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0979999997226386e+01" y="8.2870000000000005e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543462228973258e-01" /> + </geometry> + <geometry s="1.0000000000000000e-03" x="1.0980000026465104e+01" y="8.2869000000000767e+01" hdg="4.7124766973111445e+00" length="8.9535976393864303e+00"> + <arc curvature="1.7543462228973258e-01" /> + </geometry> + <geometry s="8.9545976393864297e+00" x="1.6680484875075951e+01" y="7.7169370884176772e+01" hdg="6.2832477173111441e+00" length="1.0000000000000000e-03"> + <spiral curvStart="1.7543462228973258e-01" curvEnd="0.0000000000000000e+00" /> + </geometry> + <geometry s="8.9555976393864292e+00" x="1.6681484875068303e+01" y="7.7169371005065116e+01" hdg="6.2833354346222885e+00" length="4.4000000049663406e+00"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_13_1_22_-1" length="1.5867000801409723e+01" id="1015" junction="4"> + <link> + <predecessor elementType="road" elementId="13" contactPoint="start" /> + <successor elementType="road" elementId="22" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="6.7069999999999993e+01" hdg="1.5707889799999997e+00" length="7.4202901048536773e-05"> + <line /> + </geometry> + <geometry s="7.4202901048536773e-05" x="1.0980000000545154e+01" y="6.7070074202901040e+01" hdg="1.5707889799999997e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9010163724782427e-02" /> + </geometry> + <geometry s="1.0742029010485368e-03" x="1.0980000024393643e+01" y="6.7071074202900647e+01" hdg="1.5707394749181374e+00" length="1.5864926598508676e+01"> + <arc curvature="-9.9010163724782427e-02" /> + </geometry> + <geometry s="1.5866000801409724e+01" x="2.1080473206577260e+01" y="7.7170473186599196e+01" hdg="-4.9505081862299960e-05" length="1.0000000000000000e-03"> + <spiral curvStart="-9.9010163724782427e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_9_-1_10_-1" length="1.5800000000103461e+01" id="1016" junction="3"> + <link> + <predecessor elementType="road" elementId="9" contactPoint="end" /> + <successor elementType="road" elementId="10" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="5.2590000000052989e+01" y="1.0296003993652639e+02" hdg="3.1415899999999999e+00" length="1.5800000000103461e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects> + <object s="4.2499999999999999e-01" t="-6.8499999999999996e+00" id="4007" name="parking7" dynamic="no" orientation="none" zOffset="0.0000000000000000e+00" validLength="0.0000000000000000e+00" type="parkingSpace" hdg="0.0000000000000000e+00" pitch="0.0000000000000000e+00" roll="0.0000000000000000e+00" length="8.6999999999999993e+00" width="4.7000000000000002e+00" height="2.0000000000000000e+00"> + <parkingSpace access="all" restrictions="none"> + <marking side="rear" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="front" type="solid" width="1.4999999999999999e-01" color="standard" /> + <marking side="right" type="solid" width="1.4999999999999999e-01" color="standard" /> + </parkingSpace> + </object> + </objects> + <signals /> + </road> + <road name="junction_10_1_9_1" length="1.5800000000103461e+01" id="1017" junction="3"> + <link> + <predecessor elementType="road" elementId="10" contactPoint="start" /> + <successor elementType="road" elementId="9" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="3.6789999999999999e+01" y="1.0295999999999999e+02" hdg="6.2831700000000001e+00" length="1.5800000000103461e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_23_-1_9_1" length="1.5824577584342926e+01" id="1018" junction="3"> + <link> + <predecessor elementType="road" elementId="23" contactPoint="end" /> + <successor elementType="road" elementId="9" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="4.2520964712849398e+01" y="9.2884999953536152e+01" hdg="1.5707000000000000e+00" length="7.1293661841558276e-03"> + <line /> + </geometry> + <geometry s="7.1293661841558276e-03" x="4.2520965399598389e+01" y="9.2892129319687228e+01" hdg="1.5707000000000000e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9313657022795335e-02" /> + </geometry> + <geometry s="8.1293661841558285e-03" x="4.2520965512477460e+01" y="9.2893129319680753e+01" hdg="1.5706503431714887e+00" length="1.5815448218158771e+01"> + <arc curvature="-9.9313657022795335e-02" /> + </geometry> + <geometry s="1.5823577584342926e+01" x="5.2590473333978316e+01" y="1.0296076800668715e+02" hdg="-3.9656828511258624e-05" length="1.0000000000000000e-03"> + <spiral curvStart="-9.9313657022795335e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_23_-1_10_-1" length="1.3348422063760626e+01" id="1019" junction="3"> + <link> + <predecessor elementType="road" elementId="23" contactPoint="end" /> + <successor elementType="road" elementId="10" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="4.2520964712849398e+01" y="9.2884999953536152e+01" hdg="1.5707000000000000e+00" length="4.3430496791307425e+00"> + <line /> + </geometry> + <geometry s="4.3430496791307425e+00" x="4.2521383064904420e+01" y="9.7228049612517637e+01" hdg="1.5707000000000000e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7447795480299652e-01" /> + </geometry> + <geometry s="4.3440496791307428e+00" x="4.2521383132151556e+01" y="9.7229049612515041e+01" hdg="1.5707872389774016e+00" length="9.0033723846298841e+00"> + <arc curvature="1.7447795480299652e-01" /> + </geometry> + <geometry s="1.3347422063760627e+01" x="3.6789515249613551e+01" y="1.0296048476929037e+02" hdg="3.1416772389774015e+00" length="1.0000000000000000e-03"> + <spiral curvStart="1.7447795480299652e-01" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_13_-1_14_-1" length="1.5799999999999990e+01" id="1024" junction="5"> + <link> + <predecessor elementType="road" elementId="13" contactPoint="end" /> + <successor elementType="road" elementId="14" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0979999994548946e+01" y="5.2899999999999991e+01" hdg="4.7123889800000001e+00" length="1.5799999999999990e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_14_1_13_1" length="1.5799999999999990e+01" id="1025" junction="5"> + <link> + <predecessor elementType="road" elementId="14" contactPoint="start" /> + <successor elementType="road" elementId="13" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="motorway" /> + <planView> + <geometry s="0.0000000000000000e+00" x="1.0980000000000000e+01" y="3.7100000000000001e+01" hdg="1.5707889799999997e+00" length="1.5799999999999990e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_18_-1_13_1" length="1.5867008814246613e+01" id="1026" junction="5"> + <link> + <predecessor elementType="road" elementId="18" contactPoint="end" /> + <successor elementType="road" elementId="13" contactPoint="end" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="2.1080000000055417e+01" y="4.2800041767503345e+01" hdg="3.1415899999999999e+00" length="1.4277635133908007e-04"> + <line /> + </geometry> + <geometry s="1.4277635133908007e-04" x="2.1079857223704078e+01" y="4.2800041767882213e+01" hdg="3.1415899999999999e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9011300583814563e-02" /> + </geometry> + <geometry s="1.1427763513390801e-03" x="2.1078857223704368e+01" y="4.2800041787037692e+01" hdg="3.1415404943497078e+00" length="1.5864866037895275e+01"> + <arc curvature="-9.9011300583814563e-02" /> + </geometry> + <geometry s="1.5866008814246614e+01" x="1.0979526811441305e+01" y="5.2900473204341438e+01" hdg="1.5707394743497076e+00" length="1.0000000000000000e-03"> + <spiral curvStart="-9.9011300583814563e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_18_-1_14_-1" length="1.3355545807755462e+01" id="1027" junction="5"> + <link> + <predecessor elementType="road" elementId="18" contactPoint="end" /> + <successor elementType="road" elementId="14" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="2.1080000000055417e+01" y="4.2800041767503345e+01" hdg="3.1415899999999999e+00" length="4.3999314291379683e+00"> + <line /> + </geometry> + <geometry s="4.3999314291379683e+00" x="1.6680068570932939e+01" y="4.2800053443116475e+01" hdg="3.1415899999999999e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543741706715577e-01" /> + </geometry> + <geometry s="4.4009314291379686e+00" x="1.6679068570933634e+01" y="4.2800053416530496e+01" hdg="3.1416777187085336e+00" length="8.9536143786174947e+00"> + <arc curvature="1.7543741706715577e-01" /> + </geometry> + <geometry s="1.3354545807755464e+01" x="1.0979515147553306e+01" y="3.7099515119601108e+01" hdg="4.7124766987085334e+00" length="1.0000000000000000e-03"> + <spiral curvStart="1.7543741706715577e-01" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_22_-1_21_-1" length="1.5867146698236754e+01" id="1028" junction="6"> + <link> + <predecessor elementType="road" elementId="22" contactPoint="end" /> + <successor elementType="road" elementId="21" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="3.6820000000000000e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="3.8853595896171100e-09"> + <line /> + </geometry> + <geometry s="3.8853595896171100e-09" x="3.6820000003885362e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9007658123908465e-02" /> + </geometry> + <geometry s="1.0000038853595896e-03" x="3.6821000003885118e+01" y="7.7169999983498727e+01" hdg="-4.9503829061954224e-05" length="1.5865146694351395e+01"> + <arc curvature="-9.9007658123908465e-02" /> + </geometry> + <geometry s="1.5866146698236754e+01" x="4.6920728800891588e+01" y="6.7069526800330266e+01" hdg="-1.5708205238290616e+00" length="1.0000000000000000e-03"> + <spiral curvStart="-9.9007658123908465e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_22_-1_23_-1" length="8.9555152901052466e+00" id="1029" junction="6"> + <link> + <predecessor elementType="road" elementId="22" contactPoint="end" /> + <successor elementType="road" elementId="23" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="3.6820000000000000e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543906284793165e-01" /> + </geometry> + <geometry s="1.0000000000000000e-03" x="3.6820999999999231e+01" y="7.7170000029239844e+01" hdg="8.7719531423965832e-05" length="8.9529662009279125e+00"> + <arc curvature="1.7543906284793165e-01" /> + </geometry> + <geometry s="8.9539662009279120e+00" x="4.2520484847899709e+01" y="8.2869935794149555e+01" hdg="1.5707877195314239e+00" length="1.0000000000000000e-03"> + <spiral curvStart="1.7543906284793165e-01" curvEnd="0.0000000000000000e+00" /> + </geometry> + <geometry s="8.9549662009279114e+00" x="4.2520484798027283e+01" y="8.2870935794147968e+01" hdg="1.5708754390628479e+00" length="5.4908917733520468e-04"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_22_-1_24_-1" length="1.5799999999999997e+01" id="1030" junction="6"> + <link> + <predecessor elementType="road" elementId="22" contactPoint="end" /> + <successor elementType="road" elementId="24" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="3.6820000000000000e+01" y="7.7170000000000002e+01" hdg="0.0000000000000000e+00" length="1.5799999999999997e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_21_-1_18_-1" length="1.5867012301500518e+01" id="1034" junction="7"> + <link> + <predecessor elementType="road" elementId="21" contactPoint="end" /> + <successor elementType="road" elementId="18" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="4.6919999994548945e+01" y="5.2899999999999991e+01" hdg="4.7123889800000001e+00" length="2.6810557756107301e-05"> + <line /> + </geometry> + <geometry s="2.6810557756107301e-05" x="4.6919999994548938e+01" y="5.2899973189442235e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="-9.9010426507907062e-02" /> + </geometry> + <geometry s="1.0268105577561073e-03" x="4.6919999978046818e+01" y="5.2898973189442479e+01" hdg="4.7123394747867460e+00" length="1.5864985490942763e+01"> + <arc curvature="-9.9010426507907062e-02" /> + </geometry> + <geometry s="1.5866012301500518e+01" x="3.6819526797095953e+01" y="4.2799526814853344e+01" hdg="3.1415404947867458e+00" length="1.0000000000000000e-03"> + <spiral curvStart="-9.9010426507907062e-02" curvEnd="0.0000000000000000e+00" /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_21_-1_19_-1" length="1.5799999999999990e+01" id="1035" junction="7"> + <link> + <predecessor elementType="road" elementId="21" contactPoint="end" /> + <successor elementType="road" elementId="19" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="4.6919999994548945e+01" y="5.2899999999999991e+01" hdg="4.7123889800000001e+00" length="1.5799999999999990e+01"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <road name="junction_21_-1_20_-1" length="8.9555976470302028e+00" id="1036" junction="7"> + <link> + <predecessor elementType="road" elementId="21" contactPoint="end" /> + <successor elementType="road" elementId="20" contactPoint="start" /> + </link> + <type s="0.0000000000000000e+00" type="town" /> + <planView> + <geometry s="0.0000000000000000e+00" x="4.6919999994548945e+01" y="5.2899999999999991e+01" hdg="4.7123889800000001e+00" length="1.0000000000000000e-03"> + <spiral curvStart="0.0000000000000000e+00" curvEnd="1.7543462228973303e-01" /> + </geometry> + <geometry s="1.0000000000000000e-03" x="4.6920000023787665e+01" y="5.2899000000000761e+01" hdg="4.7124766973111445e+00" length="8.9535976393864072e+00"> + <arc curvature="1.7543462228973303e-01" /> + </geometry> + <geometry s="8.9545976393864066e+00" x="5.2620484872398499e+01" y="4.7199370884176780e+01" hdg="6.2832477173111441e+00" length="1.0000000000000000e-03"> + <spiral curvStart="1.7543462228973303e-01" curvEnd="0.0000000000000000e+00" /> + </geometry> + <geometry s="8.9555976393864061e+00" x="5.2621484872390852e+01" y="4.7199371005065117e+01" hdg="6.2833354346222885e+00" length="7.6437958185238131e-09"> + <line /> + </geometry> + </planView> + <elevationProfile /> + <lateralProfile /> + <lanes> + <laneSection s="0.0000000000000000e+00"> + <center> + <lane id="0" type="driving" level="false"> + <link /> + </lane> + </center> + <right> + <lane id="-1" type="driving" level="false"> + <link /> + <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <speed sOffset="0.0000000000000000e+00" max="3.0000000000000000e+01" /> + </lane> + </right> + </laneSection> + </lanes> + <objects /> + <signals /> + </road> + <junction name="junction0" id="0"> + <connection id="0" incomingRoad="16" connectingRoad="17" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="1" incomingRoad="17" connectingRoad="16" contactPoint="start"> + <laneLink from="1" to="1" /> + </connection> + <connection id="2" incomingRoad="19" connectingRoad="16" contactPoint="end"> + <laneLink from="-1" to="1" /> + </connection> + <connection id="3" incomingRoad="19" connectingRoad="17" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + </junction> + <junction name="junction1" id="1"> + <connection id="0" incomingRoad="17" connectingRoad="0" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="1" incomingRoad="0" connectingRoad="17" contactPoint="start"> + <laneLink from="1" to="1" /> + </connection> + <connection id="2" incomingRoad="20" connectingRoad="17" contactPoint="end"> + <laneLink from="-1" to="1" /> + </connection> + <connection id="3" incomingRoad="20" connectingRoad="0" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + </junction> + <junction name="junction2" id="2"> + <connection id="0" incomingRoad="8" connectingRoad="9" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="1" incomingRoad="9" connectingRoad="8" contactPoint="start"> + <laneLink from="1" to="1" /> + </connection> + <connection id="2" incomingRoad="24" connectingRoad="8" contactPoint="end"> + <laneLink from="-1" to="1" /> + </connection> + <connection id="3" incomingRoad="24" connectingRoad="9" contactPoint="start"> + <laneLink from="-1" to="-1" /> + </connection> + </junction> + <junction name="junction3" id="3"> + <connection id="0" incomingRoad="9" connectingRoad="10" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="1" incomingRoad="10" connectingRoad="9" contactPoint="start"> + <laneLink from="1" to="1" /> + </connection> + <connection id="2" incomingRoad="23" connectingRoad="9" contactPoint="end"> + <laneLink from="-1" to="1" /> + </connection> + <connection id="3" incomingRoad="23" connectingRoad="10" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + </junction> + <junction name="junction4" id="4"> + <connection id="0" incomingRoad="12" connectingRoad="13" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="1" incomingRoad="13" connectingRoad="12" contactPoint="start"> + <laneLink from="1" to="1" /> + </connection> + <connection id="2" incomingRoad="12" connectingRoad="22" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="3" incomingRoad="13" connectingRoad="22" contactPoint="start"> + <laneLink from="1" to="-1" /> + </connection> + </junction> + <junction name="junction5" id="5"> + <connection id="0" incomingRoad="13" connectingRoad="14" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="1" incomingRoad="14" connectingRoad="13" contactPoint="start"> + <laneLink from="1" to="1" /> + </connection> + <connection id="2" incomingRoad="18" connectingRoad="13" contactPoint="end"> + <laneLink from="-1" to="1" /> + </connection> + <connection id="3" incomingRoad="18" connectingRoad="14" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + </junction> + <junction name="junction6" id="6"> + <connection id="0" incomingRoad="22" connectingRoad="21" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="1" incomingRoad="22" connectingRoad="23" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="2" incomingRoad="22" connectingRoad="24" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + </junction> + <junction name="junction7" id="7"> + <connection id="0" incomingRoad="21" connectingRoad="18" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="1" incomingRoad="21" connectingRoad="19" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + <connection id="2" incomingRoad="21" connectingRoad="20" contactPoint="end"> + <laneLink from="-1" to="-1" /> + </connection> + </junction> +</OpenDRIVE>