diff --git a/doc/opendrive_road.md b/doc/opendrive_road.md
index 1e9dfe0b036f3af710dfa13cf4f8a761b7cc5ce4..aa55a50ec3ef6ffdd0df9b0c40a36ff435c54ae7 100644
--- a/doc/opendrive_road.md
+++ b/doc/opendrive_road.md
@@ -5,37 +5,61 @@ COpendriveRoad
This class describes the whole road as a set of [COpendriveRoadSegment](opendrive_road_segment.md) objects. This class handles the creation and destruction of all road elements (road segments, lanes and road node) when the opendrive file is loaded. Once loaded, it is possible to access all the elements and information of the road, but its impossible to modify it.
+## Example road
+
+The road shown in the next figure will be used to complete the description of all classes with an example. The opendrive road descrption can be downloaded [here]().
+
+<img src="images/original_road.png" alt="Opendrive road description used as an example" width="814" height="594">
+
+The dotted lines represent the limits of adjacent road segments. Each road segment include its name and the opendrive segment identifier (in parenthesis). The name of the two road junctions are also displayed but the name of each junction road is omitted for clarity. The names of the road junctions follows the next format:
+
+```
+<junction_name>_<origin_road_id>_<origin_lane_id>_<target_road_id>_<target_lane_id>
+```
+
## Main features
The main features of this class are:
* Load the Opendrive description from a file. At the moment the only supported Opendrive version is the 1.4.
* Provides access to all the road segments of the road defined as [COpendriveRoadSegment](opendrive_road_segment.md) objects. The road segments can be accessed by several different ways:
- * **by index**: which is an arbitray number assigned when the road is loaded and coincides with the order in which the road segments are described in the Opendrive file. This approach may be usefull to cycle through all the segment to gather global information.
+ * **by index**: which is a unique arbitrary number assigned when the road is loaded and coincides with the order in which the road segments are described in the Opendrive file. This approach may be useful to cycle through all the segment to gather global information.
* **by id**: which is the numerical identifier assigned in the Opendrive file.
* **by name**: which is the string identifier assigned in the Opendrive file.
- * **by the closest position**: in this case when the position of interest is known (i.e. the position of the car, an obstacle, etc...), it is possible to get the road segment which is closest to this position. It is possible to get the index of the road segment of interest or a reference to it.
+ * **by the closest pose**: in this case when the position of interest is known (i.e. the position of the car, an obstacle, etc...), it is possible to get the road segment which is closest to this position. It is possible to get the index of the road segment of interest or a reference to it.
* Provides access to all the road lanes of the road defined as [COpendriveLane](opendrive_lane.md) objects. The road lanes can be accessed by two different ways:
- * **by index**: which is an arbitray number assigned when the road is loaded. This approach may be usefull to cycle through all the lanes to gather global information.
- * **by the closest position**: in this case when the position of interest is known (i.e. the position of the car, an obstacle, etc...), it is possible to get the road lane which is closest to this position. It is possible to get the index of the road lane of interest or a reference to it.
+ * **by index**: which is a unique arbitrary number assigned when the road is loaded. This approach may be useful to cycle through all the lanes to gather global information.
+ * **by the closest pose**: in this case when the position of interest is known (i.e. the position of the car, an obstacle, etc...), it is possible to get the road lane which is closest to this position. It is possible to get the index of the road lane of interest or a reference to it.
* Provides access to all the road nodes of the road defined as [COpendriveRoadNode](opendrive_road_node.md) objects. The road nodes can be accessed by two different ways:
- * **by index**: which is an arbitray number assigned when the road is loaded. This approach may be usefull to cycle through all the lanes to gather global information.
- * **by the closest position**: in this case when the position of interest is known (i.e. the position of the car, an obstacle, etc...), it is possible to get the road node which is closest to this position. It is possible to get the index of the road node of interest or a reference to it.
-* It is also possible to find the closest position (or positions) to the road independently of the road element where it belongs. In this case, a maximum distance parameter can be set to collect more than one candidate.
-* It is possible to generate a new road object as a subset of the original road, which are completelly independenat from each other (that is ....). There exist three possible ways to generate a subroad:
- * **by copy**: in this case the new road is just a copy of the original one.
- * **by path**: in this case the new road only has the road elements that include a set of road nodes (a path). This method generates a road with a single track.
- * **by distance**: in this case the new road only has the road elements that are inside a range from a given position, following the direction of the traffic (that is ....). This method only works with road with a single track.
+ * **by index**: which is a unique arbitrary number assigned when the road is loaded. This approach may be useful to cycle through all the lanes to gather global information.
+ * **by the closest pose**: in this case when the position of interest is known (i.e. the position of the car, an obstacle, etc...), it is possible to get the road node which is closest to this position. It is possible to get the index of the road node of interest or a reference to it.
+* It is possible to find the closest position to the road independently of the road element where it belongs.
+* It is possible to get a set of poses that are closer to a given pose than a given distance threshold, independently of the road element where they belong.
+candidate.
+* It is possible to generate a new road object as a subset of the original road, which are completely independent from each other. See the [sub-roads](#sub_roads) section for further details.
+* In case of unexpected errors, this class throws an instance of CException. All other errors are reported to the user.
For further details on the implementation of these features check the doxygen API documentation.
## Unsupported Opendrive features
-Not all features of the Opendrive standrad are supported by the current implementation. The limitations are listed next:
+Not all features of the Opendrive standard are supported by the current implementation. The limitations are listed here:
* Each road segment can have only one lane section, which implies that the number of left and right lanes is the same for all the segment.
* Each lane must have the same width for each segment.
+## Sub-roads
+
+One important feature of this class is the possibility to create new roads with a sub-set of the original road elements. The original road elements are created when the Opendrive file is loaded and can not be modified afterwards. When a sub-road is generated, new elements are created while keeping the required original information and connectivity, resulting in two completely independent objects. 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.
+
+Three different ways to create a new sub-road are supported:
+
+* copy: in this case the new road is just a copy of the original one.
+* path: in this case the new road only has the road elements that include a set of road nodes (a path).
+* distance: in this case the new road only has the road elements that within a distance to a given pose, following the direction of the traffic.
+The next images shows an example of sub-road generation for the last two cases.
-* They all have a parameter to indicate the desired heading tolerance when searching for the closest element.
diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index b38b32a2b4466377072908d346549f22436c98ef..4d3966e90a859c8d64739ecd88f2a4a44a140957 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -23,7 +23,6 @@ class COpendriveRoad
double scale_factor;
double min_road_length;
protected:
- COpendriveRoadSegment *get_segment_by_id(std::string &id);
void free(void);
void link_segments(OpenDRIVE &open_drive);
unsigned int add_node(COpendriveRoadNode *node);
@@ -66,7 +65,7 @@ class COpendriveRoad
* \brief Sets the resolution
*
* This functions sets the default resolution used to generate all the internal
- * geomtry. This functions automatically changes the resolution of all underlying
+ * geometry. This functions automatically changes the resolution of all underlying
* objects.
*
* \param res desired value of the resolution in meters. The default resolution
@@ -77,7 +76,7 @@ class COpendriveRoad
* \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 withou actually
+ * 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.
*
@@ -134,6 +133,30 @@ class COpendriveRoad
* \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
*
@@ -187,24 +210,220 @@ class COpendriveRoad
* \return constant reference to the desired road segment object.
*/
const COpendriveRoadSegment &operator[](std::size_t index);
- /**
- * \brief gets the closest node to a given point
+ /**
+ * \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.
*
- * This functions searches all the road nodes for the one which is closest to
- * a given position and returns it. The distance from the closest node to the
+ * \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;
- void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1) const;
- std::vector<unsigned int> get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road);
+ /**
+ * \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();
};
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index e7d6236174cbf26fe5886d36fa14596ed7d876e8..bb740b7e84fbb7af6e122ac865ee184e15139723 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -49,20 +49,6 @@ COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
}
-COpendriveRoadSegment *COpendriveRoad::get_segment_by_id(std::string &id)
-{
- std::stringstream error;
-
- for(unsigned int i=0;i<this->segments.size();i++)
- {
- if(this->segments[i]->get_id()==(unsigned int)std::stoi(id))
- return this->segments[i];
- }
-
- error << "No road segment with the " << id << " ID";
- throw CException(_HERE_,error.str());
-}
-
void COpendriveRoad::free(void)
{
for(unsigned int i=0;i<this->segments.size();i++)
@@ -95,7 +81,7 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it)
{
// get current segment
- road=this->get_segment_by_id(road_it->id().get());
+ 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())
{
@@ -120,13 +106,13 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
{
if(!predecessor_id.empty())
{
- prev_road=this->get_segment_by_id(predecessor_id);
+ 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=this->get_segment_by_id(successor_id);
+ next_road=const_cast<COpendriveRoadSegment *>(&this->get_segment_by_id((unsigned int)std::stoi(successor_id)));
road->link_segment(next_road);
successor_id.clear();
}
@@ -155,8 +141,8 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
}
if(predecessor_id.compare(incoming_road_id)==0 && successor_id.compare(connecting_road_id)==0)// this is the connection
{
- prev_road=this->get_segment_by_id(predecessor_id);
- next_road=this->get_segment_by_id(successor_id);
+ 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;
@@ -620,6 +606,34 @@ const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) con
}
}
+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();
@@ -795,14 +809,14 @@ double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
return closest_length;
}
-void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
+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();
- closest_lengths.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);
@@ -819,13 +833,13 @@ void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOp
if(!already_added)
{
closest_poses.push_back(poses[j]);
- closest_lengths.push_back(lengths[j]);
+ distances.push_back(lengths[j]);
}
}
}
}
-std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int> &path_nodes,TOpendriveWorldPose &start_pose,TOpendriveWorldPose &end_pose,COpendriveRoad &new_road)
+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;
@@ -914,13 +928,14 @@ void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length
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,*next_segment;
+ const COpendriveRoadSegment *segment;
COpendriveRoadSegment *new_segment;
double rem_length=length+2.0*margin,distance,start_length;
int node_side;
- bool valid;
new_road.free();
new_road.set_resolution(this->resolution);
@@ -939,10 +954,18 @@ void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length
rem_length=distance-margin;
if(rem_length<0.0)// get the previous segment
{
- next_segment=&segment->get_prev_segment(node_side,valid);
- if(valid)
+ 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
{
- segment=next_segment;
+ node_side=cand_sides[0];
+ segment=cand_segments[0];
if(node_side<0)
{
int_pose=segment->get_pose_at(segment->get_length()+rem_length);
@@ -954,11 +977,6 @@ void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length
distance=-rem_length;
}
}
- else
- {
- int_pose=segment->get_pose_at(0.0);
- distance=0.0;
- }
}
else
{
@@ -971,10 +989,18 @@ void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length
rem_length=distance+margin-segment->get_length();
if(rem_length>0.0)// get the prev segment
{
- next_segment=&segment->get_prev_segment(node_side,valid);
- if(valid)
+ 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
{
- segment=next_segment;
+ node_side=cand_sides[0];
+ segment=cand_segments[0];
if(node_side<0)
{
int_pose=segment->get_pose_at(segment->get_length()-rem_length);
@@ -986,11 +1012,6 @@ void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length
distance=rem_length;
}
}
- else
- {
- int_pose=segment->get_pose_at(segment->get_length());
- distance=0.0;
- }
}
else
{
@@ -1003,40 +1024,46 @@ void COpendriveRoad::get_sub_road(TOpendriveWorldPose &start_pose,double &length
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);
- valid=true;
if(new_segment->get_length()<this->min_road_length)
{
- next_segment=&segment->get_next_segment(node_side,valid);
- segment=next_segment;
delete new_segment;
- if(valid)
+ 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;
}
- if(valid)
+ start_length=new_segment->get_length()-distance;
+ if(rem_length<start_length)
{
- 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;
+ 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);
}
- while(rem_length>this->resolution && valid)
+ rem_length-=new_segment->get_length();
+ new_road.add_segment(new_segment);
+ new_segment_ref[(COpendriveRoadSegment *)segment]=new_segment;
+ while(rem_length>this->resolution)
{
- next_segment=&segment->get_next_segment(node_side,valid);
- segment=next_segment;
- if(valid)
+ 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)