diff --git a/include/opendrive/opendrive_road.h b/include/opendrive/opendrive_road.h
deleted file mode 100644
index 58e68e24e1be697ddc2952aadfc0b406e926e40b..0000000000000000000000000000000000000000
--- a/include/opendrive/opendrive_road.h
+++ /dev/null
@@ -1,467 +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"
-#include "opendrive_junction.h"
-
-class COpendriveRoad
-{
- friend class COpendriveRoadSegment;
- friend class COpendriveRoadNode;
- friend class COpendriveLane;
- friend class COpendriveJunction;
- private:
- std::vector<COpendriveRoadSegment *> segments;
- std::vector<COpendriveRoadNode *> nodes;
- std::vector<COpendriveLane *> lanes;
- std::vector<COpendriveJunction *> junctions;
- double resolution;
- double scale_factor;
- double min_road_length;
- protected:
- void free(void);
- void link_segments(OpenDRIVE &open_drive);
- void link_neighbors(OpenDRIVE &open_drive);
- void add_segment(const COpendriveRoadSegment *segment,segment_up_ref_t &new_segment_ref,node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref);
- 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,link_up_ref_t &new_link_ref);
- void reindex(void);
- void prune(std::vector<unsigned int> &path_nodes);
- unsigned int create_junction(const std::string &name,unsigned int start_segment,int start_lane_id,unsigned int end_segment,int end_lane_id);
- 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);
- void save(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;
- unsigned int get_num_junctions(void) const;
- const COpendriveJunction &get_junction(unsigned int index) const;
- const COpendriveJunction &get_junction_by_id(unsigned int id) const;
- const COpendriveJunction &get_junction_by_name(std::string &name) 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 Returns a road node by id
- *
- * This function returns a constant reference to the desired road node
- * 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 unique identifier assigend to the node when it was created.
- *
- * \return constant reference to the desired road node object.
- */
- const COpendriveRoadNode& get_node_by_id(unsigned int id) 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,bool on_road=false) 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,bool on_road=false) 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,bool on_road=false) 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,bool on_road=false) 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,bool on_road=false) const;
- /**
- * \brief returns the closest road segment to a given pose with the given node
- */
- const COpendriveRoadSegment &get_closest_segment_with_node(const COpendriveRoadNode &node,TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1,bool on_road=false) 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,bool on_road=false) const;
- /**
- * \brief returns the index of the closest road segment to a given pose
- */
- unsigned int get_closest_segment_index_with_node(const COpendriveRoadNode &node,TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1,bool on_road=false) 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.
- *
- */
- std::vector<unsigned int> get_sub_road(TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose,double &length,COpendriveRoad &new_road,std::vector<unsigned int> &path_nodes,int initial_side=0);
- double get_pose_distance(TOpendriveWorldPose &pose,int road_side,bool on_road=true);
- /**
- * \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);
- unsigned int add_segment(COpendriveRoadSegment &segment);
- unsigned int add_junction(COpendriveJunction &junction);
- void link_segments_by_index(unsigned int start_segment,unsigned int end_segment);
- void link_segments_by_name(const std::string &start_segment,const std::string &end_segment);
- void link_segments_by_id(unsigned int start_segment,unsigned int end_segment);
- /**
- * \brief Destructor
- */
- ~COpendriveRoad();
-};
-
-#endif
diff --git a/src/examples/opendrive_test.cpp b/src/examples/opendrive_test.cpp
deleted file mode 100644
index b5f6d5c7a2faf3d43091e45960b41ece9eebf748..0000000000000000000000000000000000000000
--- a/src/examples/opendrive_test.cpp
+++ /dev/null
@@ -1,241 +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,new_path,new_path2;
- TOpendriveWorldPose end_pose,start_pose;
- double length=100.0;
-// std::string xml_file="/home/shernand/Downloads/map.xodr";
-// std::string xml_file="/home/shernand/Downloads/osm2xodr_old/output.xodr";
-// std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/add.xodr";
- std::string xml_file="/home/shernand/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/build/new_road.xodr";
-// std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/adc.xodr";
-// std::string xml_file="/home/shernand/iri-lab/iri_team_ws/src/iri_adc_launch/data/adc/adc.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;
-
-// return 0;
-/*
- for(unsigned int i=0;i<road.get_num_nodes();i++)
- {
- const COpendriveRoadNode &node=road.get_node(i);
- TOpendriveWorldPose pose=node.get_pose();
- std::cout << pose.x << "," << pose.y << "," << pose.heading << std::endl;
- }
-
- return 0;
-*/
- 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.clear();
- path.push_back(51);
- path.push_back(34);
- path.push_back(55);
- path.push_back(0);
- path.push_back(2);
- path.push_back(4);
- path.push_back(6);
- path.push_back(47);
- path.push_back(8);
- path.push_back(49);
- path.push_back(10);
- path.push_back(12);
- path.push_back(14);
- path.push_back(16);
- path.push_back(59);
- path.push_back(18);
- end_pose.x=60.2742;
- end_pose.y=105.16;
- end_pose.heading=3.14159;
- start_pose.x=52.2;
- start_pose.y=15.0;
- start_pose.heading=0.0;
- new_path=road.get_sub_road(path,end_pose,new_road);
- new_path2=new_road.get_sub_road(start_pose,end_pose,length,new_road2,new_path,-1);
-
- 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;
- }
- }
- return 0;
- /*
- path.clear();
- path.push_back(27);
- path.push_back(29);
- path.push_back(89);
- 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(135);
- path.push_back(58);
- path.push_back(131);
- path.push_back(24);
- path.push_back(103);
- path.push_back(3);
- path.push_back(99);
- path.push_back(1);
- path.push_back(95);
- path.push_back(96);
- path.push_back(32);
- path.push_back(33);
- end_pose.x=0.0;
- end_pose.y=0.0;
- end_pose.heading=3.14159;
- start_pose.x=0.0;
- start_pose.y=0.0;
- start_pose.heading=0.0;
- road.get_sub_road(path,end_pose,new_road,false);
- new_road.get_sub_road(start_pose,length,1.0,new_road2);
- */
-// std::cout << "road2" << std::endl;
-// std::cout << new_road << 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;
- }
- }
-
- return 0;
-*/
-// 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=-2.2;
- start_pose.heading=0.0;
- end_pose.x=0.0;
- end_pose.y=2.2;
- end_pose.heading=3.14159;
- length=100.0;
- new_path=road.get_sub_road(path,end_pose,new_road);
- new_path=new_road.get_sub_road(start_pose,end_pose,length,new_road2,new_path);
-// std::cout << "road3" << std::endl;
-// std::cout << new_road << 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;
- }
- }
- }
- catch (CException &e)
- {
- std::cout << "[Exception caught] : " << e.what() << std::endl;
- }
- return 0;
-}
diff --git a/src/examples/osm_test.cpp b/src/examples/osm_test.cpp
deleted file mode 100644
index 33a86b3ec5c1f8eff0705c8e45db19e7ed84c585..0000000000000000000000000000000000000000
--- a/src/examples/osm_test.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-#include "osm/osm_map.h"
-#include "opendrive_road.h"
-#include "exceptions.h"
-#include <iostream>
-#include <vector>
-#include <limits>
-
-const std::string filename="/home/shernand/Downloads/map.osm";
-//const std::string filename="/home/shernand/Downloads/castelldefels.osm";
-
-int main(int argc, char *argv[])
-{
- std::vector<double> x,y,heading,curvature;
-
- try{
- COSMMap map;
- COpendriveRoad road;
-
- map.load(filename);
- map.convert_to_opendrive(road);
- road.save("./new_road.xodr");
-
- map.get_paths(x,y,heading,curvature);
- for(unsigned int i=0;i<x.size();i++)
- std::cout << x[i] << "," << y[i] << std::endl;
-
- std::cout << map << std::endl;
- }
- catch (std::exception &e)
- {
- std::cout << "[Exception caught] : " << e.what() << std::endl;
- }
- return 0;
-}
diff --git a/src/opendrive/opendrive_road.cpp b/src/opendrive/opendrive_road.cpp
deleted file mode 100644
index 5d93b250a77e61dd2d36ccd5da0f6681d3493274..0000000000000000000000000000000000000000
--- a/src/opendrive/opendrive_road.cpp
+++ /dev/null
@@ -1,1799 +0,0 @@
-#include "opendrive_road.h"
-#include "exceptions.h"
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <math.h>
-#include <chrono>
-#include <ctime>
-#include <fstream>
-
-#include <Eigen/Dense>
-
-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++)
- {
- if(this->segments[i]!=NULL)
- {
- delete this->segments[i];
- this->segments[i]=NULL;
- }
- }
- this->segments.clear();
- for(unsigned int i=0;i<this->nodes.size();i++)
- {
- if(this->nodes[i]!=NULL)
- {
- delete this->nodes[i];
- this->nodes[i]=NULL;
- }
- }
- this->nodes.clear();
- for(unsigned int i=0;i<this->lanes.size();i++)
- {
- if(this->lanes[i]!=NULL)
- {
- delete this->lanes[i];
- this->lanes[i]=NULL;
- }
- }
- this->lanes.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();
-}
-
-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;
- int junction_id,incomming_id,connecting_id;
- 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::stol(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();
- incomming_id=std::stol(predecessor_id);
- prev_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stol(predecessor_id)));
- 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();
- connecting_id=std::stol(successor_id);
- next_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stol(successor_id)));
- successor_contact=road_it->lane_link().get().successor().get().contactPoint().get();
- }
- }
- }
- junction_id=std::stol(road_it->junction().get());
- if(junction_id==-1)// non junction road segments
- {
- if(!predecessor_id.empty())
- {
- if(!prev_road->is_junction())
- prev_road->link_segment(road);
- predecessor_id.clear();
- }
- if(!successor_id.empty())
- {
- if(!next_road->is_junction())
- road->link_segment(next_road);
- successor_id.clear();
- }
- }
- else// junction segment
- {
- COpendriveJunction *junction=const_cast<COpendriveJunction *>(&this->get_junction_by_id(junction_id));
- std::vector<TJunctionConnection> connections;
- if(!junction->get_connections_between(incomming_id,connecting_id,connections))
- {
- error << "Connectivity information missing for segment " << road->get_name() << ": No incomming segment";
- throw CException(_HERE_,error.str());
- }
- for(unsigned int i=0;i<connections.size();i++)
- {
- for(unsigned int j=0;j<connections[i].links.size();j++)
- {
- TOpendriveWorldPose end;
- TOpendriveWorldPose start=road->get_lane(-1).get_start_pose();
- bool from_start,to_start;
- if(predecessor_contact=="end")
- {
- from_start=false;
- if(connections[i].links[j].from_lane_id<0)
- end=prev_road->get_lane(connections[i].links[j].from_lane_id).get_end_pose(true);
- else
- end=prev_road->get_lane(connections[i].links[j].from_lane_id).get_start_pose();
- if(fabs(end.x-start.x)>this->resolution || fabs(end.y-start.y)>this->resolution)
- continue;
- }
- else
- {
- from_start=true;
- if(connections[i].links[j].from_lane_id<0)
- end=prev_road->get_lane(connections[i].links[j].from_lane_id).get_start_pose();
- else
- end=prev_road->get_lane(connections[i].links[j].from_lane_id).get_end_pose(true);
- if(fabs(end.x-start.x)>this->resolution || fabs(end.y-start.y)>this->resolution)
- continue;
- }
- end=road->get_lane(-1).get_end_pose(true);
- if(successor_contact=="end")
- {
- to_start=false;
- if(connections[i].links[j].to_lane_id<0)
- start=next_road->get_lane(connections[i].links[j].to_lane_id).get_end_pose(true);
- else
- start=next_road->get_lane(connections[i].links[j].to_lane_id).get_start_pose();
- if(fabs(end.x-start.x)>this->resolution || fabs(end.y-start.y)>this->resolution)
- continue;
- }
- else
- {
- to_start=true;
- if(connections[i].links[j].to_lane_id<0)
- start=next_road->get_lane(connections[i].links[j].to_lane_id).get_start_pose();
- else
- start=next_road->get_lane(connections[i].links[j].to_lane_id).get_end_pose(true);
- if(fabs(end.x-start.x)>this->resolution || fabs(end.y-start.y)>this->resolution)
- continue;
- }
- prev_road->link_segment(road,connections[i].links[j].from_lane_id,from_start,-1,true);
- road->link_segment(next_road,-1,false,connections[i].links[j].to_lane_id,to_start);
- junction->add_segment(road);
- }
- }
- }
- }
-}
-
-void COpendriveRoad::link_neighbors(OpenDRIVE &open_drive)
-{
- COpendriveRoadSegment *road,*neighbor_road,*road1,*road2;
- const COpendriveLane *cand1_lane,*cand1_next_lane;
- const COpendriveLane *cand2_lane,*cand2_next_lane;
- std::vector<COpendriveRoadSegment *> neighbor_candidates;
- link2::neighbor_iterator neighbor_it;
- std::stringstream error;
- std::string neighbor_id;
- bool same_direction;
-
- // import neighbors from XML file
- 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::stol(road_it->id().get())));
- // get neighbors
- if(road_it->lane_link().present())
- {
- // iterate for all neigbors
- for(neighbor_it=road_it->lane_link().get().neighbor().begin();neighbor_it!=road_it->lane_link().get().neighbor().end();neighbor_it++)
- {
- if(neighbor_it->side().present() && neighbor_it->elementId().present() && neighbor_it->direction().present())
- {
- neighbor_id=neighbor_it->elementId().get();
- if(neighbor_it->direction().get()=="same")
- same_direction=true;
- else
- same_direction=false;
- neighbor_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stol(neighbor_id)));
- if(neighbor_it->side().get()=="left")
- road->set_left_neighbor(neighbor_road,same_direction);
- else if(neighbor_it->side().get()=="right")
- road->set_right_neighbor(neighbor_road,same_direction);
- else
- {
- error << "Invalid neighbor information for segment " << road->get_name() << ": invalid side identifier";
- throw CException(_HERE_,error.str());
- }
- }
- }
- }
- }
- // find neighbors from connectivity
- for(OpenDRIVE::junction_iterator junction_it(open_drive.junction().begin());junction_it!=open_drive.junction().end();++junction_it)
- {
- for(junction::connection_iterator connection_it1(junction_it->connection().begin()); connection_it1!=junction_it->connection().end();++connection_it1)
- {
- std::string incoming_road_id1;
- std::string connecting_road_id1;
- if(connection_it1->incomingRoad().present())
- incoming_road_id1=connection_it1->incomingRoad().get();
- else
- {
- error << "Connectivity information missing for segment " << road->get_name() << ": No incomming segment";
- throw CException(_HERE_,error.str());
- }
- if(connection_it1->connectingRoad().present())
- connecting_road_id1=connection_it1->connectingRoad().get();
- else
- {
- error << "Connectivity information missing for segment " << road->get_name() << ": No connecting segment";
- throw CException(_HERE_,error.str());
- }
- for(junction::connection_iterator connection_it2(junction_it->connection().begin()+1); connection_it2!=junction_it->connection().end();++connection_it2)
- {
- std::string incoming_road_id2;
- std::string connecting_road_id2;
- if(connection_it2->incomingRoad().present())
- incoming_road_id2=connection_it2->incomingRoad().get();
- else
- {
- error << "Connectivity information missing for segment " << road->get_name() << ": No incomming segment";
- throw CException(_HERE_,error.str());
- }
- if(connection_it2->connectingRoad().present())
- connecting_road_id2=connection_it2->connectingRoad().get();
- else
- {
- error << "Connectivity information missing for segment " << road->get_name() << ": No connecting segment";
- throw CException(_HERE_,error.str());
- }
- if((incoming_road_id1==incoming_road_id2 && connecting_road_id1==connecting_road_id2) ||
- (incoming_road_id1==connecting_road_id2 && incoming_road_id2==connecting_road_id1))
- {
- road1=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stol(incoming_road_id1)));
- road2=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stol(connecting_road_id1)));
- neighbor_candidates.clear();
- 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::stol(road_it->id().get())));
- if(road->is_junction() && road->connects_segments(road1,road2))
- neighbor_candidates.push_back(road);
- }
- for(unsigned int i=0;i<neighbor_candidates.size();i++)
- {
- if(neighbor_candidates[i]->get_num_left_lanes()!=0)// not a regular junction road
- continue;
- if(neighbor_candidates[i]->get_num_right_lanes()!=1)// not a regular junction road
- continue;
- cand1_lane=&neighbor_candidates[i]->get_lane(-1);
- if(cand1_lane->get_num_next_lanes()!=1)// not a regular junction road
- continue;
- cand1_next_lane=&cand1_lane->get_next_lane(0);
- for(unsigned int j=1;j<neighbor_candidates.size();j++)
- {
- if(neighbor_candidates[j]->get_num_left_lanes()!=0)// not a regular junction road
- continue;
- if(neighbor_candidates[j]->get_num_right_lanes()!=1)// not a regular junction road
- continue;
- cand2_lane=&neighbor_candidates[j]->get_lane(-1);
- if(cand2_lane->get_num_next_lanes()!=1)// not a regular junction road
- continue;
- cand2_next_lane=&cand2_lane->get_next_lane(0);
- if(&cand1_next_lane->get_segment()==&cand2_next_lane->get_segment())
- {
- if(fabs(cand1_next_lane->get_id()-cand2_next_lane->get_id())==1)
- {
- if(fabs(cand1_next_lane->get_id())<fabs(cand2_next_lane->get_id()))
- {
- neighbor_candidates[i]->set_right_neighbor(neighbor_candidates[j],true);
- neighbor_candidates[j]->set_left_neighbor(neighbor_candidates[i],true);
- }
- else
- {
- neighbor_candidates[j]->set_right_neighbor(neighbor_candidates[i],true);
- neighbor_candidates[i]->set_left_neighbor(neighbor_candidates[j],true);
- }
- }
- }
- else
- {
- if(cand1_next_lane->get_id()*cand2_next_lane->get_id()==-1)
- {
- neighbor_candidates[j]->set_left_neighbor(neighbor_candidates[i],false);
- neighbor_candidates[i]->set_left_neighbor(neighbor_candidates[j],false);
- }
- }
- }
- }
- }
- }
- }
- }
-}
-
-void COpendriveRoad::add_segment(const COpendriveRoadSegment *segment,segment_up_ref_t &new_segment_ref,node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref)
-{
- std::vector<const COpendriveRoadSegment *> next_segments,prev_segments,cand_prev_segments;
- std::map<int,COpendriveRoadSegment *> junction_segments;
- std::vector<TJunctionConnection> connections;
- std::vector<int> next_sides,prev_sides;
- COpendriveRoadSegment *new_segment;
- std::stringstream road_name;
- int lane_id,inner_lane_id=std::numeric_limits<int>::max();
- unsigned int num_right_lanes=0,num_left_lanes=0;
- double offset;
-
- if(segment->is_junction())
- {
- next_segments=segment->get_next_segments(-1,next_sides);
- prev_segments=segment->get_prev_segments(-1,prev_sides);
- if(next_segments.size()!=1 || prev_segments.size()!=1)
- return;
- for(unsigned int i=0;i<segment->parent_road->get_num_segments();i++)
- {
- COpendriveRoadSegment *candidate_segment=segment->parent_road->segments[i];
- if(candidate_segment->is_junction() && candidate_segment->get_junction().get_id()==segment->get_junction().get_id())
- {
- if(candidate_segment->connects_segments((COpendriveRoadSegment *)next_segments[0],(COpendriveRoadSegment *)prev_segments[0]))
- {
- cand_prev_segments=candidate_segment->get_prev_segments(-1,prev_sides);
- if(prev_segments[0]==cand_prev_segments[0])
- {
- const COpendriveRoadNode &node=candidate_segment->get_node(0);
- for(unsigned int j=0;j<node.get_num_links();j++)
- {
- if(node.get_link(j).segment==prev_segments[0])
- if(node.get_link(j).lane!=NULL)
- {
- lane_id=node.get_link(j).lane->get_id();
- break;
- }
- }
- junction_segments[lane_id]=candidate_segment;
- if(lane_id<0 && (abs(lane_id) < abs(inner_lane_id)))
- inner_lane_id=lane_id;
- if(lane_id>0 && ((unsigned int)lane_id)>num_left_lanes)
- num_left_lanes=lane_id;
- else if(lane_id<0 && ((unsigned int)-lane_id)>num_right_lanes)
- num_right_lanes=-lane_id;
- }
- else
- {
- const COpendriveRoadNode &node=candidate_segment->get_node(candidate_segment->get_num_nodes()-1);
- for(unsigned int j=0;j<node.get_num_links();j++)
- {
- if(node.get_link(j).segment==next_segments[0])
- if(node.get_link(j).lane!=NULL)
- {
- lane_id=node.get_link(j).lane->get_id();
- break;
- }
- }
- junction_segments[lane_id]=candidate_segment;
- if(lane_id<0 && (abs(lane_id) < abs(inner_lane_id)))
- inner_lane_id=lane_id;
- if(lane_id>0 && ((unsigned int)lane_id)>num_left_lanes)
- num_left_lanes=lane_id;
- else if(lane_id<0 && ((unsigned int)-lane_id)>num_right_lanes)
- num_right_lanes=-lane_id;
- }
- }
- }
- }
- if(junction_segments.size()==1)
- {
- new_segment=junction_segments.begin()->second->clone(new_node_ref,new_lane_ref,new_link_ref);
- this->add_segment(new_segment);
- new_segment_ref[junction_segments.begin()->second]=new_segment;
- }
- else
- {
- road_name << "junction" << segment->get_junction().get_id() << "_road";
- new_segment=new COpendriveRoadSegment(road_name.str(),OD_MARK_BROKEN,segment->get_id(),false);
- // get the inner most lane
- const COpendriveRoadSegment *inner_segment=junction_segments[inner_lane_id];
- for(unsigned int i=0;i<inner_segment->geometries.size();i++)
- new_segment->add_geometry(inner_segment->geometries[i].opendrive);
- offset=0.0;
- for(unsigned int i=1;i<=num_right_lanes;i++)
- {
- new_segment->lanes[-i]=junction_segments[-i]->lanes[-1];
- new_segment->lanes[-i]->id=-i;
- new_segment->lanes[-i]->offset=-offset;
- offset+=junction_segments[-i]->lanes[-1]->width;
- for(unsigned int j=0;j<junction_segments[-i]->lanes[-1]->get_num_nodes();j++)
- new_segment->nodes.push_back(junction_segments[-i]->lanes[-1]->nodes[j]);
- junction_segments[-i]->lanes[-1]->clone(new_node_ref,new_lane_ref,new_link_ref);
- new_segment->num_right_lanes++;
- }
- offset=0.0;
- for(unsigned int i=1;i<=num_left_lanes;i++)
- {
- new_segment->lanes[i]=junction_segments[i]->lanes[-1];
- new_segment->lanes[i]->id=i;
- new_segment->lanes[i]->offset=offset;
- offset+=junction_segments[i]->lanes[-1]->width;
- for(unsigned int j=0;j<junction_segments[i]->lanes[-1]->get_num_nodes();j++)
- new_segment->nodes.push_back(junction_segments[i]->lanes[-1]->nodes[j]);
- junction_segments[i]->lanes[-1]->clone(new_node_ref,new_lane_ref,new_link_ref);
- new_segment->num_left_lanes++;
- }
- new_segment->connecting=segment->connecting;
- this->add_segment(new_segment);
- for(std::map<int,COpendriveRoadSegment *>::iterator it=junction_segments.begin();it!=junction_segments.end();it++)
- new_segment_ref[(*it).second]=new_segment;
- }
- }
- else
- {
- new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
- this->add_segment(new_segment);
- new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment;
- }
-}
-
-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;
-// TOpendriveTrackPose track_pose;
-
- for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
- {
-// try{
-// end_pose=(*lane_it)->get_end_pose();
-// }catch(CException &e){
-// track_pose.t=(*lane_it)->get_center_offset();
-// if((*lane_it)->get_id()<0)
-// {
-// track_pose.heading=0.0;
-// track_pose.s=(*lane_it)->segment->get_length(true);
-// }
-// else
-// {
-// track_pose.s=0.0;
-// track_pose.heading=3.14159;
-// }
- end_pose=(*lane_it)->get_end_pose(true);
-// }
- 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();
- }
- }
-}
-
-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];
- (*lane_it)->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];
- (*node_it)->update_references(segment_refs,lane_refs,node_refs);
- }
-}
-
-void COpendriveRoad::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs,link_up_ref_t &new_link_ref)
-{
- 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,new_link_ref);
- 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)->clean_references(segment_refs,lane_refs,node_refs,new_link_ref);
- 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)->clean_references(segment_refs,lane_refs,node_refs,new_link_ref);
- node_it++;
- }
- else
- node_it=this->nodes.erase(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)
-{
- COpendriveRoadSegment *lane_segment;
- std::vector<COpendriveLane *>::iterator lane_it,tmp_it;
- int lane_id;
-
- // for each point
- for(unsigned int i=0;i<path_nodes.size();i++)
- {
- // find the lane where it belongs
- for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
- {
- if((*lane_it)->has_node_with_index(path_nodes[i]))
- break;
- }
- // lane_it has the lane where the node belongs
- lane_segment=(*lane_it)->segment;
- lane_id=(*lane_it)->get_id();
- for(tmp_it=this->lanes.begin();tmp_it!=this->lanes.end();)
- {
- if((*tmp_it)->segment==lane_segment)//belongs to the same segment)
- {
- if(lane_id*(*tmp_it)->get_id()<0)// opposite direction
- {
-
- if(abs((*tmp_it)->get_id())==1)
- {
- lane_segment->remove_lane(*tmp_it);
- for(unsigned int j=0;j<(*tmp_it)->nodes.size();j++)
- this->remove_node((*tmp_it)->nodes[j]);
- tmp_it=this->lanes.erase(tmp_it);
- }
- else
- {
- lane_segment->remove_lane(*tmp_it);
- for(unsigned int j=0;j<(*tmp_it)->nodes.size();j++)
- this->remove_node((*tmp_it)->nodes[j]);
- tmp_it=this->lanes.erase(tmp_it);
- }
- }
- else
- tmp_it++;
- }
- else
- tmp_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]);
- else if(this->nodes[i]->links[j]->next==NULL)
- this->nodes[i]->remove_link(this->nodes[i]->links[j]);
- }
- }
-}
-
-unsigned int COpendriveRoad::create_junction(const std::string &name,unsigned int start_segment,int start_lane_id,unsigned int end_segment,int end_lane_id)
-{
- TOpendriveWorldPose start_pose,end_pose,end_pose_zero,end_pose_rot,end_pose_new;
- Eigen::MatrixXd A=Eigen::MatrixXd::Zero(6,8),pinv;
- Eigen::VectorXd b=Eigen::VectorXd::Zero(6,1),sol;
- TOpendrivePoly3Params u_params,v_params;
- COpendriveLane new_lane(4.0,50.0,OD_MARK_NONE);
- COpendriveRoadSegment *new_segment;
- COpendriveParamPoly3 *poly_geom;
- bool from_start,to_start;
- unsigned int index;
-
- if(start_segment == (unsigned int)-1)
- throw CException(_HERE_,"Invalid start segment id");
- if(end_segment == (unsigned int)-1)
- throw CException(_HERE_,"Invalid end segment id");
- if(start_segment==end_segment)
- throw CException(_HERE_,"Can not link a segment to itself");
- COpendriveRoadSegment *start_seg=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id(start_segment));
- start_pose=start_seg->get_lane(start_lane_id).get_end_pose(true);
- start_pose.x+=-2.0*std::sin(start_pose.heading);
- start_pose.y+=2.0*std::cos(start_pose.heading);
- if(start_lane_id<0)
- from_start=false;
- else
- from_start=true;
- COpendriveRoadSegment *end_seg=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id(end_segment));
- end_pose=end_seg->get_lane(end_lane_id).get_start_pose();
- end_pose.x+=-2.0*std::sin(end_pose.heading);
- end_pose.y+=2.0*std::cos(end_pose.heading);
- if(end_lane_id<0)
- to_start=true;
- else
- to_start=false;
- 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;
- u_params.a=sol(0);
- u_params.b=sol(1);
- u_params.c=sol(2);
- u_params.d=sol(3);
- v_params.a=sol(4);
- v_params.b=sol(5);
- v_params.c=sol(6);
- v_params.d=sol(7);
- poly_geom=new COpendriveParamPoly3(start_pose,u_params,v_params);
- end_pose_new=poly_geom->get_end_pose();
- if(fabs(end_pose.x-end_pose_new.x)>0.001 || fabs(end_pose.y-end_pose_new.y)>0.001 || fabs(end_pose.heading-end_pose_new.heading)>0.001)
- {
-// std::cout << "junction not created: target (" << end_pose.x << "," << end_pose.y << "," << end_pose.heading << ") actual (" << end_pose_new.x << "," << end_pose_new.y << "," << end_pose_new.heading << ")" << std::endl;
- delete poly_geom;
- return -1;
- }
- new_segment=new COpendriveRoadSegment(name,OD_MARK_NONE,-1,true);
- new_segment->add_right_lane(new_lane);
- new_segment->add_geometry(poly_geom);
- index=this->add_segment(*new_segment);
- start_seg->link_segment(this->segments[index],start_lane_id,from_start,-1,true);
- this->segments[index]->link_segment(end_seg,-1,false,end_lane_id,to_start);
- delete new_segment;
- delete poly_geom;
-
- return index;
-}
-
-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)<this->resolution && std::abs(pose.y-node_pose.y)<this->resolution)
- 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)<this->resolution && std::abs(pose.y-node_pose.y)<this->resolution)
- 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;
- }
- }
- for (OpenDRIVE::junction_iterator junction_it(open_drive->junction().begin()); junction_it != open_drive->junction().end(); ++junction_it)
- {
- try{
- COpendriveJunction *junction=new COpendriveJunction();
- junction->set_parent_road(this);
- junction->load(*junction_it);
- this->junctions.push_back(junction);
- }catch(CException &e){
- std::cout << e.what() << std::endl;
- }
- }
- // link segments
- this->link_segments(*open_drive);
- this->link_neighbors(*open_drive);
- this->complete_open_lanes();
- }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::save(const std::string &filename)
-{
- xml_schema::namespace_infomap map;
- ::header opendrive_header;
- ::OpenDRIVE *opendrive_data;
-
- 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->segments.size();i++)
- {
- OpenDRIVE::road_type *new_road;
- this->segments[i]->save(&new_road);
- opendrive_data->road().push_back(*new_road);
- delete new_road;
- }
- for(unsigned int i=0;i<this->junctions.size();i++)
- {
- OpenDRIVE::junction_type new_junction;
- this->junctions[i]->save(new_junction);
- opendrive_data->junction().push_back(new_junction);
- }
- map[""].name="";
- map[""].schema=".OpenDRIVE_1.4Hxsd";
- 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 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_junctions(void) const
-{
- return this->junctions.size();
-}
-
-const COpendriveJunction &COpendriveRoad::get_junction(unsigned int index) const
-{
- std::stringstream error;
-
- if(index>=0 && index<this->junctions.size())
- return *this->junctions[index];
- else
- {
- error << "Invalid junction index " << index;
- throw CException(_HERE_,error.str());
- }
-}
-
-const COpendriveJunction &COpendriveRoad::get_junction_by_id(unsigned int id) const
-{
- std::stringstream error;
-
- for(unsigned int i=0;i<this->junctions.size();i++)
- {
- if(this->junctions[i]->get_id()==id)
- return *this->junctions[i];
- }
-
- error << "No road junction with the " << id << " ID";
- throw CException(_HERE_,error.str());
-}
-
-const COpendriveJunction &COpendriveRoad::get_junction_by_name(std::string &name) const
-{
- std::stringstream error;
-
- for(unsigned int i=0;i<this->junctions.size();i++)
- {
- if(this->junctions[i]->get_name()==name)
- return *this->junctions[i];
- }
-
- error << "No road junctions with the " << name << " name";
- throw CException(_HERE_,error.str());
-}
-
-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 COpendriveRoadNode& COpendriveRoad::get_node_by_id(unsigned int id) const
-{
- std::stringstream error;
-
- for(unsigned int i=0;i<this->nodes.size();i++)
- {
- if(this->nodes[i]->get_index()==id)
- return *this->nodes[i];
- }
-
- error << "Invalid node id " << id;
- 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,bool on_road) const
-{
- double dist,min_dist=std::numeric_limits<double>::max(),length,lane_width;
- TOpendriveWorldPose closest_pose;
- unsigned int closest_index=-1,num_roads=0;
-
- 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)
- {
- if(on_road)
- {
- for(unsigned int j=0;j<this->nodes[i]->parents.size();j++)
- {
- lane_width=this->nodes[i]->parents[j].lane->get_width();
- if(dist<(lane_width/2.0))
- num_roads++;
- }
- if(num_roads>0)
- {
- min_dist=dist;
- closest_index=i;
- distance=length;
- }
- }
- else
- {
- min_dist=dist;
- closest_index=i;
- distance=length;
- }
- }
- }
- }
-
- return closest_index;
-}
-
-const COpendriveRoadNode &COpendriveRoad::get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol,bool on_road) const
-{
- unsigned int closest_index;
-
- closest_index=this->get_closest_node_index(pose,distance,angle_tol,on_road);
- 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,bool on_road) const
-{
- double dist,min_dist=std::numeric_limits<double>::max(),length,lane_width;
- 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)
- {
- if(on_road)
- {
- lane_width=this->lanes[i]->get_width();
- if(dist<(lane_width/2.0))
- {
- min_dist=dist;
- closest_index=i;
- distance=length;
- }
- }
- else
- {
- min_dist=dist;
- closest_index=i;
- distance=length;
- }
- }
- }
- }
-
- return closest_index;
-}
-
-const COpendriveLane &COpendriveRoad::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol,bool on_road) const
-{
- unsigned int closest_index;
-
- closest_index=this->get_closest_lane_index(pose,distance,angle_tol,on_road);
- 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,bool on_road) const
-{
- double dist,min_dist=std::numeric_limits<double>::max(),length,road_width;
- 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)
- {
- if(on_road)
- {
- if(this->segments[i]->get_pose_side(pose)<0)
- road_width=this->segments[i]->get_right_width();
- else
- road_width=this->segments[i]->get_left_width();
- if(dist<road_width)
- {
- min_dist=dist;
- closest_index=i;
- distance=length;
- }
- }
- else
- {
- min_dist=dist;
- closest_index=i;
- distance=length;
- }
- }
- }
- }
-
- return closest_index;
-}
-
-unsigned int COpendriveRoad::get_closest_segment_index_with_node(const COpendriveRoadNode &node,TOpendriveWorldPose &pose,double &distance,double angle_tol,bool on_road) const
-{
- double dist,min_dist=std::numeric_limits<double>::max(),length,road_width;
- 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())
- {
- for(unsigned int j=0;j<node.get_num_parents();j++)
- {
- if(&node.get_parent_segment(j)==this->segments[i])
- {
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
- {
- if(on_road)
- {
- if(this->segments[i]->get_pose_side(pose)<0)
- road_width=this->segments[i]->get_right_width();
- else
- road_width=this->segments[i]->get_left_width();
- if(dist<road_width)
- {
- min_dist=dist;
- closest_index=i;
- distance=length;
- }
- }
- else
- {
- min_dist=dist;
- closest_index=i;
- distance=length;
- }
- }
- }
- }
- }
- }
-
- return closest_index;
-}
-
-const COpendriveRoadSegment &COpendriveRoad::get_closest_segment(TOpendriveWorldPose &pose,double &distance,double angle_tol,bool on_road) const
-{
- unsigned int closest_index;
-
- closest_index=this->get_closest_segment_index(pose,distance,angle_tol,on_road);
- if(closest_index==(unsigned int)-1)
- throw CException(_HERE_,"Impossible to find the closest segment");
- return *this->segments[closest_index];
-}
-
-const COpendriveRoadSegment &COpendriveRoad::get_closest_segment_with_node(const COpendriveRoadNode &node,TOpendriveWorldPose &pose,double &distance,double angle_tol,bool on_road) const
-{
- unsigned int closest_index;
-
- closest_index=this->get_closest_segment_index_with_node(node,pose,distance,angle_tol,on_road);
- 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=std::numeric_limits<double>::max();
-
- 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;
- 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_road.add_segment(segment,new_segment_ref,new_node_ref,new_lane_ref,new_link_ref);
- }
- // 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_road.add_segment(segment,new_segment_ref,new_node_ref,new_lane_ref,new_link_ref);
-
- new_road.update_references(new_segment_ref,new_lane_ref,new_node_ref);
-// new_road.prune(path_nodes);
- new_road.clean_references(new_segment_ref,new_lane_ref,new_node_ref,new_link_ref);
- new_road.reindex();
- new_road.complete_open_lanes();
-
- return new_road.update_path(new_node_ref,path_nodes);
-}
-
-std::vector<unsigned int> COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose,double &length,COpendriveRoad &new_road,std::vector<unsigned int> &path_nodes,int initial_side)
-{
- segment_up_ref_t new_segment_ref;
- lane_up_ref_t new_lane_ref,tmp_lane_ref;
- node_up_ref_t new_node_ref,tmp_node_ref;
- link_up_ref_t new_link_ref,tmp_link_ref;
- std::vector<unsigned int>::iterator path_it;
- TOpendriveWorldPose int_end_pose,closest_start_pose;
- unsigned int end_segment_index;
- const COpendriveRoadSegment *segment,*end_segment;
- COpendriveRoadSegment *new_segment;
- double rem_length=length,distance,end_distance,start_length;
- int node_side,end_segment_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
- for(path_it=path_nodes.begin();path_it!=path_nodes.end();)
- {
- try{
- const COpendriveRoadNode &node=this->get_node_by_id(*path_it);
- segment=&this->get_closest_segment_with_node(node,start_pose,distance,6.28318);
- break;
- }catch(CException &e){
- path_it=path_nodes.erase(path_it);
- }
- }
- node_side=segment->get_pose_side(start_pose);
- // get the last segment
- end_segment_index=this->get_closest_segment_index(end_pose,end_distance,6.28318);
- if(end_segment_index==(unsigned int)-1)
- throw CException(_HERE_,"End position does not belong to the road");
- end_segment=this->segments[end_segment_index];
- end_segment_side=end_segment->get_pose_side(end_pose);
- node_side=initial_side;
- rem_length=length;
- if(segment==end_segment && node_side==end_segment_side)
- {
- new_segment=segment->get_sub_segment(tmp_node_ref,tmp_lane_ref,tmp_link_ref,node_side,&start_pose,&end_pose);
- if(new_segment->get_length()<this->min_road_length)
- throw CException(_HERE_,"Road too short");
- }
- else
- {
- new_segment=segment->get_sub_segment(tmp_node_ref,tmp_lane_ref,tmp_link_ref,node_side,&start_pose,NULL);
- if(new_segment->get_length()<this->min_road_length)
- {
- delete new_segment;
- segment=segment->get_next_path_segment(path_nodes,node_side,node_side);
- path_nodes.erase(path_nodes.begin());
- tmp_node_ref=new_node_ref;
- tmp_lane_ref=new_lane_ref;
- tmp_link_ref=new_link_ref;
- if(segment==end_segment && node_side==end_segment_side)
- new_segment=segment->get_sub_segment(tmp_node_ref,tmp_lane_ref,tmp_link_ref,node_side,NULL,&end_pose);
- else
- new_segment=segment->clone(tmp_node_ref,tmp_lane_ref,tmp_link_ref);
- }
- }
- start_length=new_segment->get_closest_pose(start_pose,closest_start_pose,6.28318);
- if(node_side>0)
- start_length=new_segment->get_length()-start_length;
- new_node_ref=tmp_node_ref;
- new_lane_ref=tmp_lane_ref;
- new_link_ref=tmp_link_ref;
- rem_length-=(new_segment->get_length()-start_length);
- new_road.add_segment(new_segment);
- new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment;
- while((segment!=end_segment || node_side!=end_segment_side) && rem_length>this->resolution)
- {
- segment=segment->get_next_path_segment(path_nodes,node_side,node_side);
- if((rem_length-segment->get_length())<this->resolution)
- {
-// if((rem_length-segment->get_length())>-2.0)
-// new_segment=segment->clone(new_node_ref,new_lane_ref,new_link_ref);
-// else
-// {
- int_end_pose=segment->get_pose_at(rem_length,node_side);
- new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&int_end_pose);
-// }
- new_road.add_segment(new_segment);
- new_segment_ref[(COpendriveRoadSegment *)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,new_link_ref);
- new_road.reindex();
- new_road.complete_open_lanes();
- rem_length-=new_segment->get_length();
- length-=rem_length;
- length+=start_length;
- return new_road.update_path(new_node_ref,path_nodes);
- }
- else if(segment==end_segment && node_side==end_segment_side)
- {
- new_segment=segment->get_sub_segment(new_node_ref,new_lane_ref,new_link_ref,node_side,NULL,&end_pose);
- rem_length-=new_segment->get_length();
- new_road.add_segment(new_segment);
- new_segment_ref[(COpendriveRoadSegment *)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,new_link_ref);
- new_road.reindex();
- new_road.complete_open_lanes();
- length-=rem_length;
- length+=start_length;
- return new_road.update_path(new_node_ref,path_nodes);
- }
- else
- {
- tmp_node_ref=new_node_ref;
- tmp_lane_ref=new_lane_ref;
- tmp_link_ref=new_link_ref;
- new_segment=segment->clone(tmp_node_ref,tmp_lane_ref,tmp_link_ref);
- }
- if(new_segment->get_length()>this->min_road_length)
- {
- new_node_ref=tmp_node_ref;
- new_lane_ref=tmp_lane_ref;
- new_link_ref=tmp_link_ref;
- rem_length-=new_segment->get_length();
- new_road.add_segment(new_segment);
- new_segment_ref[(COpendriveRoadSegment *)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,new_link_ref);
- new_road.reindex();
- new_road.complete_open_lanes();
- length-=rem_length;
- length+=start_length;
-
- return new_road.update_path(new_node_ref,path_nodes);
-}
-
-double COpendriveRoad::get_pose_distance(TOpendriveWorldPose &pose,int road_side,bool on_road)
-{
- const COpendriveRoadSegment *current_segment;
- std::vector<const COpendriveRoadSegment *> cand_segments;
- std::vector<int> cand_sides;
- std::stringstream error;
- double distance;
-
- try{
- current_segment=&this->get_closest_segment(pose,distance,6.283185307,on_road);
- if(road_side>0)
- distance=current_segment->get_length()-distance;
- }catch(CException &e){
- return std::numeric_limits<double>::max();
- }
- do{
- cand_segments=current_segment->get_prev_segments(road_side,cand_sides);
- if(cand_segments.size()==1)
- {
- current_segment=cand_segments[0];
- road_side=cand_sides[0];
- distance+=current_segment->get_length();
- }
- }while(cand_segments.size()==1);
-
- return distance;
-}
-
-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];
- for(unsigned int i=0;i<road.junctions.size();i++)
- std::cout << *road.junctions[i];
-
- return out;
-}
-
-unsigned int COpendriveRoad::add_segment(COpendriveRoadSegment &segment)
-{
- for(unsigned int i=0;i<this->segments.size();i++)
- if(this->segments[i]->get_id()==segment.get_id())
- throw CException(_HERE_,"Segment already present in road");
- COpendriveRoadSegment *new_segment=new COpendriveRoadSegment(segment);
- new_segment->set_resolution(this->resolution);
- new_segment->set_scale_factor(this->scale_factor);
- new_segment->set_min_road_length(this->min_road_length);
- new_segment->set_parent_road(this);
- new_segment->add_lanes_to_parent();
- new_segment->update();
- this->segments.push_back(new_segment);
-
- return this->segments.size()-1;
-}
-
-unsigned int COpendriveRoad::add_junction(COpendriveJunction &junction)
-{
- std::stringstream name;
- unsigned int index;
-
- for(unsigned int i=0;i<this->junctions.size();i++)
- if(this->junctions[i]->get_id()==junction.get_id())
- throw CException(_HERE_,"Junction already present in road");
- COpendriveJunction *new_junction=new COpendriveJunction(junction);
- new_junction->set_parent_road(this);
- for(unsigned int i=0;i<junction.get_num_connections();i++)
- {
- const TJunctionConnection &connection=junction.get_connection(i);
- for(unsigned int j=0;j<connection.links.size();j++)
- {
- name.str("");
- name << "junction_" << connection.incomming_road_id << "_" << connection.links[j].from_lane_id << "_" << connection.connecting_road_id << "_" << connection.links[j].to_lane_id;
- index=this->create_junction(name.str(),connection.incomming_road_id,connection.links[j].from_lane_id,connection.connecting_road_id,connection.links[j].to_lane_id);
- if(index==(unsigned int)-1)
- new_junction->delete_link_from_connection(i,j);
- else
- new_junction->add_segment(this->segments[index]);
- }
- }
-
- this->junctions.push_back(new_junction);
-
- return this->junctions.size()-1;
-}
-
-void COpendriveRoad::link_segments_by_index(unsigned int start_segment,unsigned int end_segment)
-{
- if(start_segment == (unsigned int)-1 || start_segment>this->segments.size())
- throw CException(_HERE_,"Invalid start segment index");
- if(end_segment == (unsigned int)-1 || end_segment>this->segments.size())
- throw CException(_HERE_,"Invalid end segment index");
- if(start_segment==end_segment)
- throw CException(_HERE_,"Can not link a segment to itself");
- this->segments[start_segment]->link_segment(this->segments[end_segment]);
-}
-
-void COpendriveRoad::link_segments_by_name(const std::string &start_segment,const std::string &end_segment)
-{
- unsigned int start_segment_index=-1,end_segment_index=-1;
-
- for(unsigned int i=0;i<this->segments.size();i++)
- {
- if(this->segments[i]->get_name()==start_segment)
- start_segment_index=i;
- if(this->segments[i]->get_name()==end_segment)
- end_segment_index=i;
- }
- this->link_segments_by_index(start_segment_index,end_segment_index);
-}
-
-void COpendriveRoad::link_segments_by_id(unsigned int start_segment,unsigned int end_segment)
-{
- unsigned int start_segment_index=-1,end_segment_index=-1;
-
- for(unsigned int i=0;i<this->segments.size();i++)
- {
- if(this->segments[i]->get_id()==start_segment)
- start_segment_index=i;
- if(this->segments[i]->get_id()==end_segment)
- end_segment_index=i;
- }
- this->link_segments_by_index(start_segment_index,end_segment_index);
-}
-
-COpendriveRoad::~COpendriveRoad()
-{
- this->free();
- this->resolution=DEFAULT_RESOLUTION;
- this->scale_factor=DEFAULT_SCALE_FACTOR;
- this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
-}
-