diff --git a/doc/images/ref_system_lanes.png b/doc/images/ref_system_lanes.png
index 596940da24a0c15b26ef61e2feecab5182a55629..e03e2b1e49a67fef9062e56d01afc393d04aa499 100644
Binary files a/doc/images/ref_system_lanes.png and b/doc/images/ref_system_lanes.png differ
diff --git a/doc/opendrive_lane.md b/doc/opendrive_lane.md
index 1114b21e047f8f26abed8019ff3498e4b3a7b23c..93e007d8dcdc66a0ec059f6fb6a0bf1533ab1169 100644
--- a/doc/opendrive_lane.md
+++ b/doc/opendrive_lane.md
@@ -11,35 +11,41 @@ The next two images show (as transparent green boxes) the set of COpendriveLane'
<img src="images/lanes_junction.png" alt="COpendriveLane objects created for a junction of the example road" width="622" height="457">
-Each lane is assigned an index when it is created which depends only on the order in which they are imported from the Opendrive file (black number in the middle of the green boxes in the previous image). The lanes are also identified in the same way that Opendrive identifies them: negative integers values are used to identify rigth lanes, and positive values to identify left lanes. These values increase (for left lanes) or decrease (for right lanes) the further they are from the center lane of the segment (value in parenthesis in the previous images).
+Each lane is assigned a unique index when it is created which depends only on the order in which they are imported from the Opendrive file (black number in the middle of the green boxes in the previous image). This index may only change when a new sub-road is generated, in which case the lanes in the new road are re-indexed. The lanes are also identified in the same way that Opendrive identifies them: negative integers values are used to identify right lanes, and positive values to identify left lanes. These values increase (for left lanes) or decrease (for right lanes) the further they are from the center lane of the segment (value in parenthesis in the previous images).
-The second image shows a detail of the COpendriveLane objects created for lower junction (juntion0). The upper junction (juntion1) is equivalent.
+The second image shows a detail of the COpendriveLane objects created for the lower junction (junction0). The upper junction (junction1) is equivalent.
+
+The lanes are defined in the direction of the traffic. That is, right lanes have the same heading as the road segment, but left lanes have opposite heading, starting at the end of the road segment and ending at the start.
+
+The lanes can only have a constant width. Multiple width records on the opendirve road description file are not supported. Also, one speed record is required for each lane. If no speed is defined a default speed of 60Km/h will be used. Check the [main opendrive documentation](opendirve) for more information on the supported features.
## Main features
The main features of this class are:
-* The lane is defined in the direction of the traffic. That is, right lanes have the same heading as the road segment, but left lanes have opposite heading, starting at the end of the roas segmetn and endint at the start.
-* Provides access to all the road nodes of the lane defined as [COpendriveRoadNode](). The road nodes can be accessed by two different ways:
- * **by index**: which is an arbitray number assigned when the road is loaded.
- * **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
-* It is also possible to find the closest position (or positions) to the road independently of the road element where it belongs.
-* Provides connectivity information of each lane with its neightbours. The connecting lanes can only be accessed by identifier.
-* Provides a referecne to the parent road segment [COpendriveRoadSegment]().
-* Funcions per retornar punts de la corva (pose i curvatura) en funció de la distà ncia.
-* The road marks to the left and right sides of the road are imported from the Opendrive file.
+* Provides access to all the road nodes of the lane defined as [COpendriveRoadNode](opendrive_road_node.md). The road nodes can be accessed by two different ways:
+ * **by local index**: which is the local index of the road node inside the lane, not the unique index assigned to the road nodes when they are created.
+ * **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.
+* Provides connectivity information of each lane with its neighbours (previous, next, left and right lanes). The Previous and next connecting lanes can only be accessed by their local index, not the one assigned to the lanes when they are created.
+* Provides road mark information for the left and right sides, even if the left or right lanes don't exist.
+* Provides a reference to the parent road segment [COpendriveRoadSegment](opendrive_road_segment.md).
+* It is possible to find the closest position to the lane independently of the road element where it belongs.
+* It is possible to get the pose and curvature at any point on the lane trajectory.
+* 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.
-## Reference systems
+## Closest nodes, poses and lane distances
-At this point it is important to pay some attention to the reference systems used by lanes and how the closest points and distances are computed.
+At this point it is important to pay some attention to how the closest nodes, poses and lane distances are computed.
In this case, the origin of the reference system is placed at the center of the lane, at the beginning of the lane. For right lanes this means the origin is placed at the beginning of the road segment, and for left lanes it means that the origin is at the end of the road segment. The positive x axis (in red) is pointing toward the segment and the positive y axis (in green) is always pointing to the left (the positive z axis would be up). The next figure shows the reference system for left and right lanes for two road segment (a straight segment on the left and a curve segment on the right).
-<img src="images/ref_system_lanes.png" alt="Definition of the reference system used for COpendriveLane objects" width="989" height="369">
+<img src="images/ref_system_lanes.png" alt="Closest poses and node distances computation for road nodes" width="1023" height="382">
+
+The lane geometry is build from the lane link geometries of the links connecting the road nodes belonging to the lane. Therefore, the computation of the closest pose is equivalent to that of the links. See the [COpendrveLink](opendrive_link.md) documentation for more details.
-When finding the closest point to a lane, the desired position of interest (yellow points in the previous image) is projected (closest distance) to the geometry that describes the center of the lane (light red points in the previous image labeled R or L). If the imaginary line between the position of interest and its projection into the segment is not orthogonal to the lane geometry, it is considered that the point of interest does not belong to the lane, and therefore, the closest point does not exist.
+The closest road node to a given pose is the road node belonging to the lane whose lane link has the smallest euclidean distance to the given pose. Since the closest pose to a link is computed with respect to its starting road node, the closest road node will always be the one before the closest point, even though the distance to the next road node in the lane may be smaller. This case is shown in previous image, where the closest road nodes are shown in blue.
-In case the closest point exist, its distance is always computed from the origin of the reference system of the lane, following the path defined by the lane geometry (center of the lane) as shown in the previous image.
+In case the closest point exist, its **lane distance** is always computed from the origin of the reference system of the lane, following the path defined by the lane geometry (center of the lane) as shown in the previous image. Do not confuse this distance with the closest euclidean distance between the point of interest and the closest point belonging to the lane.
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index e03e4908a66c8979e5477f608e97591e2aefe1ef..7142db8ed184b77b5fb5458bbee32caf836f2fac 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -60,31 +60,323 @@ class COpendriveLane
void update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *end=NULL);
COpendriveLane *get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
COpendriveLane *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref);
+ TOpendriveLocalPose transform_to_local_pose(TOpendriveTrackPose &track) const;
+ TOpendriveWorldPose transform_to_world_pose(TOpendriveTrackPose &track) const;
public:
+ /**
+ * \brief Returns the number of road nodes in the lane
+ *
+ * This function returns how many road nodes belong to the lane.
+ *
+ * \return This function returns the number of road nodes. The range of
+ * valid indexes goes from 0 to the value returned by this function -1.
+ */
unsigned int get_num_nodes(void) const;
+ /**
+ * \brief Returns a road node by local index
+ *
+ * This function returns a constant reference to the desired road node
+ * object, which can not be modified by the user. If the index is not valid,
+ * this function will throw an exception.
+ *
+ * \param index is the 0 based local index of the desired road node. It is not
+ * the unique index assigned to the road nodes when they are created.
+ *
+ * \return constant reference to the desired road node object.
+ */
const COpendriveRoadNode &get_node(unsigned int index) const;
+ /**
+ * \brief Returns a reference to the parent road segment
+ *
+ * This function returns a constant reference to the road segment where the lane
+ * belongs, which can not be modified by the user. If the parent segment is not
+ * defined, this function will throw an exception.
+ *
+ * \return constant reference to the parent road segment.
+ */
const COpendriveRoadSegment &get_segment(void) const;
+ /**
+ * \brief Returns the number of next lanes
+ *
+ * This function returns how many lanes are connected to this one in the direction
+ * of the traffic (the next lanes).
+ *
+ * \return This function returns the number of next lanes. The range of
+ * valid indexes goes from 0 to the value returned by this function -1.
+ */
unsigned int get_num_next_lanes(void) const;
+ /**
+ * \brief Returns a reference to a next lane
+ *
+ * This function returns a constant reference to a next lane connected to this one,
+ * which can not be modified by the user. If the index is not valid, this function
+ * will throw an exception.
+ *
+ * \param index is the 0 based local index of the desired next lane. It is not
+ * the unique index assigned to the lanes when they are created.
+ *
+ * \return constant reference to a next lane.
+ */
const COpendriveLane &get_next_lane(unsigned int index) const;
+ /**
+ * \brief Returns the number of previous lanes
+ *
+ * This function returns how many lanes are connected to this one in the opposite
+ * direction of the traffic (the previous lanes).
+ *
+ * \return This function returns the number of previous lanes. The range of
+ * valid indexes goes from 0 to the value returned by this function -1.
+ */
unsigned int get_num_prev_lanes(void) const;
+ /**
+ * \brief Returns a reference to a previous lane
+ *
+ * This function returns a constant reference to a previos lane connected to this one,
+ * which can not be modified by the user. If the index is not valid, this function
+ * will throw an exception.
+ *
+ * \param index is the 0 based local index of the desired previous lane. It is not
+ * the unique index assigned to the lanes when they are created.
+ *
+ * \return constant reference to a previous lane.
+ */
const COpendriveLane &get_prev_lane(unsigned int index) const;
+ /**
+ * \brief Checks whether there exist a left lane or not
+ *
+ * This function checks if the lane has an adjacent left lane. Adjacent lanes are
+ * on the same side of the road. Lanes of opposite sides of the road are not
+ * considered adjacent.
+ *
+ * \return true if the lane has an adjacent left lane or false otherwise.
+ */
+ bool exist_left_lane(void);
+ /**
+ * \brief Returns a reference to a left lane
+ *
+ * This function returns a constant reference to a left lane adjacent to this one,
+ * which can not be modified by the user. If there is no adjacent left lane, this
+ * function will throw an exception.
+ *
+ * \return constant reference to the left lane.
+ */
+ const COpendriveLane &get_left_lane(void) const;
+ /**
+ * \brief Checks whether there exist a right lane or not
+ *
+ * This function checks if the lane has an adjacent right lane. Adjacent lanes are
+ * on the same side of the road. Lanes of opposite sides of the road are not
+ * considered adjacent.
+ *
+ * \return true if the lane has an adjacent right lane or false otherwise.
+ */
+ bool exist_right_lane(void);
+ /**
+ * \brief Returns a reference to a right lane
+ *
+ * This function returns a constant reference to a right lane adjacent to this one,
+ * which can not be modified by the user. If there is no adjacent right lane, this
+ * function will throw an exception.
+ *
+ * \return constant reference to the right lane.
+ */
+ const COpendriveLane &get_right_lane(void) const;
+ /**
+ * \brief return the road mark at the left side of the lane
+ *
+ * This function returns the road mark identifier at the left side of the lane, even
+ * if the left lane is not defined.
+ *
+ * \return the road mark identifier at the left side of the lane.
+ */
+ opendrive_mark_t get_left_road_mark(void) const;
+ /**
+ * \brief return the road mark at the right side of the lane
+ *
+ * This function returns the road mark identifier at the right side of the lane, even
+ * if the right lane is not defined.
+ *
+ * \return the road mark identifier at the right side of the lane.
+ */
+ opendrive_mark_t get_left_road_mark(void) const;
+ /**
+ * \brief Returns the width of the lane
+ *
+ * \returns the width in meters of the lane.
+ */
double get_width(void) const;
+ /**
+ * \brief Returns the speed limit of the lane
+ *
+ * \return the speed limit of the lane in km/h
+ */
double get_speed(void) const;
- double get_offset(void) const;
+ /**
+ * \brief Returns the offset from the road segment center.
+ *
+ * This function returns the offset of the center of the lane with respect of
+ * the center of the road segment where it belongs.
+ *
+ * \return the distance form the center of the segment in meters.
+ */
double get_center_offset(void) const;
+ /**
+ * \brief Returns the unique index of the lane
+ *
+ * \return the unique index assigned to the lane when it was created.
+ */
unsigned int get_index(void) const;
+ /**
+ * \brief Returns the opendrive identifier of the lane
+ *
+ * This function returns the opendrive identifier of the lane: negative integers
+ * values are used to identify right lanes, and positive values to identify left
+ * lanes. These values increase (for left lanes) or decrease (for right lanes) the
+ * further they are from the center lane of the segment.
+ *
+ * \return the opendrive identifier of the lane.
+ */
int get_id(void) const;
+ /**
+ * \brief Returns the start pose of the lane
+ *
+ * This function returns the start pose (x and y coordinates and heading) of the
+ * lane in the world reference system. The lanes are defined in the direction of
+ * the traffic. That is, right lanes have the same heading as the road segment,
+ * but left lanes have opposite heading, starting at the end of the road segment
+ * and ending at the start.
+ *
+ * \return the start pose of the lane in world coordinates.
+ */
TOpendriveWorldPose get_start_pose(void) const;
+ /**
+ * \brief Returns the end pose of the lane
+ *
+ * This function returns the end pose (x and y coordinates and heading) of the
+ * lane in the world reference system. The lanes are defined in the direction of
+ * the traffic. That is, right lanes have the same heading as the road segment,
+ * but left lanes have opposite heading, starting at the end of the road segment
+ * and ending at the start.
+ *
+ * \return the end pose of the lane in world coordinates.
+ */
TOpendriveWorldPose get_end_pose(void) const;
+ /**
+ * \brief returns the length of the lane
+ *
+ * This function returns the length of the lane geometry. The lane geometry is
+ * the same as the link geometries of the lane links connecting the road nodes
+ * belonging to the lane.
+ *
+ * \return length of the lane in meters.
+ */
double get_length(void) const;
+ /**
+ * \brief returns the pose of the lane at a given point
+ *
+ * This function returns the pose (x and y coordinates and heading) of the lane geometry
+ * at a given distance from the origin point of the link. The lane geometry is
+ * the same as the link geometries of the lane links connecting the road nodes
+ * belonging to the lane.
+ *
+ * \param length desired distance from the origin point of the lane in meters. This
+ * distance must be smaller or equal than the actual lane length.
+ *
+ * \return the pose at the desired point in the world reference system.
+ */
TOpendriveWorldPose get_pose_at(double length) const;
+ /**
+ * \brief returns the curvature of the lane at a given point
+ *
+ * This function returns the curvature of the lane geometry at a given distance from
+ * the origin point of the link. The lane geometry is the same as the link geometries
+ * of the lane links connecting the road nodes belonging to the lane.
+ *
+ * \param length desired distance from the origin point of the lane in meters. This
+ * distance must be smaller or equal than the actual link length.
+ *
+ * \return the curvature at the desired point.
+ */
double get_curvature_at(double length) const;
- TOpendriveLocalPose transform_to_local_pose(TOpendriveTrackPose &track) const;
- TOpendriveWorldPose transform_to_world_pose(TOpendriveTrackPose &track) const;
+
+ /**
+ * \brief
+ */
const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+ /**
+ * \brief returns the index of the closest node to a given pose
+ *
+ * This functions finds the closest road node belonging to the lane to a given
+ * pose, and returns its index. This function only returns a valid index
+ * when the given pose coincides with the lane.
+ *
+ * \param world given pose to find the closest link.
+ * \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 lane, following the corresponding lane link geometry.
+ * If the closest lane link does not exist this argument is set to the
+ * maximum double value (std::numeric_limits<double>::max())
+ * \angle_tol maximum heading difference between the given pose and the closest pose
+ * on the lane geometry.
+ *
+ * \return the index of the closest node or -1 if there is no close node to the given
+ * pose.
+ */
unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+ /**
+ * \brief returns the closest node to a given pose
+ *
+ * This functions finds the closest road node belonging to the lane to a given
+ * pose, and returns it. This function only returns a valid reference when the
+ * given pose coincides with the lane. Otherwise, it throws an exception.
+ *
+ * \param world given pose to find the closest link.
+ * \param distance the node distance between the projection of the given pose to
+ * the closest lane geometry, and the start position of the lane,
+ * following the lane geometry. If the closest link does not exist
+ * this argument is set to the maximum double value
+ * (std::numeric_limits<double>::max())
+ * \angle_tol maximum heading difference between the given pose and the closest pose
+ * on the lane geometry.
+ *
+ * \return a constant reference to the closest node which can not be modified by the
+ * user.
+ */
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
+ /**
+ * \brief returns the closest pose to a given pose
+ *
+ * This functions finds the closest pose (minimum distance) on the lane geometry
+ * to a given pose. The closest pose only exist if the imaginary line between the
+ * given pose and its projection into the lane is orthogonal to the lane geometry.
+ * Otherwise, it is considered that the given pose does not coincide with the lane.
+ *
+ * \param world given pose to find the closest link.
+ * \param closest_pose the closest pose to the lane if the given pose coincides with
+ * it. Otherwise the data on this parameters is not valid.
+ * \angle_tol maximum heading difference between the given pose and the closest pose
+ * on the lane.
+ *
+ * \return the lane distance between the projection of the given pose to the
+ * lane geometry, following its geometry. If the closest pose does not exist,
+ * the maximum double value (std::numeric_limits<double>::max()) will be
+ * returned.
+ */
friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
+ /**
+ * \brief overloaded stream operator
+ *
+ * This functions streams human readable information about the lane. This information
+ * includes:
+ * * The opendrive lane id.
+ * * Its width, sped limit and offset from the center of the road segment.
+ * * The parent road segment.
+ * * For each previous and next lanes: the road segment and lane information.
+ * * The left lane and the left road mark.
+ * * The right lane and the right road mark.
+ * * The road nodes information.
+ *
+ */
~COpendriveLane();
};
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 65cc184389c03bed82496103af77e1f2c4d0baf5..0df25457723f00ec87eab13add652b53b757efa7 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -617,6 +617,56 @@ const COpendriveRoadSegment &COpendriveLane::get_segment(void) const
return *this->segment;
}
+bool COpendriveLane::exist_left_lane(void)
+{
+ if(this->left_lane!=NULL)
+ return true;
+ else
+ return false;
+}
+
+const COpendriveLane &COpendriveLane::get_left_lane(void) const
+{
+ std::stringstream error;
+
+ if(this->left_lane!=NULL)
+ return *this->left_lane;
+
+ error << "Left lane not defined for segment " << this->segment->get_name() << " (lane " << this->id << ")";
+
+ throw CException(_HERE_,error.str());
+}
+
+bool COpendriveLane::exist_right_lane(void)
+{
+ if(this->right_lane!=NULL)
+ return true;
+ else
+ return false;
+}
+
+const COpendriveLane &COpendriveLane::get_right_lane(void) const
+{
+ std::stringstream error;
+
+ if(this->right_lane!=NULL)
+ return *this->right_lane;
+
+ error << "Right lane not defined for segment " << this->segment->get_name() << " (lane " << this->id << ")";
+
+ throw CException(_HERE_,error.str());
+}
+
+opendrive_mark_t COpendriveLane::get_left_road_mark(void) const
+{
+ return this->left_mark;
+}
+
+opendrive_mark_t COpendriveLane::get_right_road_mark(void) const
+{
+ return this->right_mark;
+}
+
double COpendriveLane::get_width(void) const
{
return this->width/this->scale_factor;
@@ -627,11 +677,6 @@ double COpendriveLane::get_speed(void) const
return this->speed;
}
-double COpendriveLane::get_offset(void) const
-{
- return this->offset/this->scale_factor;
-}
-
double COpendriveLane::get_center_offset(void) const
{
if(this->id<0)
@@ -857,6 +902,12 @@ std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
out << " Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
out << " width: " << lane.get_width() << std::endl;
out << " speed: " << lane.get_speed() << std::endl;
+ out << " Previous lanes: " << std::endl;
+ for(unsigned int i=0;i<previous.size();i++)
+ out << " Lane " << this->previous[i]->id << " in road segment " << this->previous[i]->segment->get_name() << std::endl;
+ out << " Next lanes: " << std::endl;
+ for(unsigned int i=0;i<next.size();i++)
+ out << " Lane " << this->next[i]->id << " in road segment " << this->next[i]->segment->get_name() << std::endl;
if(lane.segment!=NULL)
out << " Parent road segment: " << lane.segment->get_name() << std::endl;
else