diff --git a/doc/opendrive_lane.md b/doc/opendrive_lane.md
index 93e007d8dcdc66a0ec059f6fb6a0bf1533ab1169..c652fb53ec401b6713d088de9f0218440834e037 100644
--- a/doc/opendrive_lane.md
+++ b/doc/opendrive_lane.md
@@ -25,7 +25,7 @@ The main features of this class are:
* 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.
+ * **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.
* 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).
@@ -41,11 +41,11 @@ At this point it is important to pay some attention to how the closest nodes, po
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="Closest poses and node distances computation for road nodes" width="1023" height="382">
+<img src="images/ref_system_lanes.png" alt="Closest poses and lane distances computation for lanes" 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.
-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 **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.
+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.
+
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index f080f58b737c362a8cb0d8e0eb7e5d662397da72..ce8482125e3274303156202ee939649d299e782b 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -73,7 +73,7 @@ class COpendriveLane
*/
unsigned int get_num_nodes(void) const;
/**
- * \brief Returns a road node by local index
+ * \brief Returns a reference to a road node by local index
*
* This function returns a constant reference to the desired road node
* object, which can not be modified by the user. If the index is not valid,
@@ -131,7 +131,7 @@ class COpendriveLane
/**
* \brief Returns a reference to a previous lane
*
- * This function returns a constant reference to a previos lane connected to this one,
+ * This function returns a constant reference to a previous lane connected to this one,
* which can not be modified by the user. If the index is not valid, this function
* will throw an exception.
*
@@ -275,7 +275,7 @@ class COpendriveLane
* \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
+ * at a given distance from the origin point of the lane. The lane geometry is
* the same as the link geometries of the lane links connecting the road nodes
* belonging to the lane.
*
@@ -289,60 +289,55 @@ class COpendriveLane
* \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
+ * the origin point of the lane. The lane geometry is the same as the link geometries
* of the lane links connecting the road nodes belonging to the lane.
*
* \param length desired distance from the origin point of the lane in meters. This
- * distance must be smaller or equal than the actual link length.
+ * distance must be smaller or equal than the actual lane length.
*
* \return the curvature at the desired point.
*/
double get_curvature_at(double length) const;
-
- /**
- * \brief
- */
- 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
+ * \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 its index. This function only returns a valid index
- * when the given pose coincides with the lane.
+ * 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 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 lane, following the corresponding lane link geometry.
- * If the closest lane link does not exist this argument is set to the
+ * position of the node, following the corresponding lane link geometry.
+ * If the closest road node does not exist this argument is set to the
* maximum double value (std::numeric_limits<double>::max())
- * \angle_tol maximum heading difference between the given pose and the closest pose
- * on the lane 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 node or -1 if there is no close node to the given
- * pose.
+ * \return a constant reference to the closest node which can not be modified by the
+ * user.
*/
- unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+ const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
/**
- * \brief returns the closest node to a given pose
+ * \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 it. This function only returns a valid reference when the
- * given pose coincides with the lane. Otherwise, it throws an exception.
+ * 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 world given pose to find the closest road node.
* \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.
+ * the lane link geometry of the closest road node, and the start
+ * position of the node, following the corresponding lane link geometry.
+ * If the closest road node does not exist this argument is set to the
+ * maximum double value (std::numeric_limits<double>::max())
+ * \param angle_tol maximum heading difference between the given pose and the closest pose
+ * on the lane link geometry of the closest road node
*
- * \return a constant reference to the closest node which can not be modified by the
- * user.
+ * \return the index of the closest node or -1 if there is no node close to the given
+ * pose.
*/
- double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
+ unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
/**
* \brief returns the closest pose to a given pose
*
@@ -351,18 +346,18 @@ class COpendriveLane
* 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 world given pose to find the closest pose.
* \param closest_pose the closest pose to the lane if the given pose coincides with
* it. Otherwise the data on this parameters is not valid.
- * \angle_tol maximum heading difference between the given pose and the closest pose
- * on the lane.
+ * \param angle_tol maximum heading difference between the given pose and the closest pose
+ * on the lane geometry.
*
* \return the lane distance between the projection of the given pose to the
* lane geometry, following its geometry. If the closest pose does not exist,
* the maximum double value (std::numeric_limits<double>::max()) will be
* returned.
*/
- friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
+ double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
/**
* \brief overloaded stream operator
*
@@ -377,6 +372,10 @@ class COpendriveLane
* * The road nodes information.
*
*/
+ friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
+ /**
+ * brief Destructor
+ */
~COpendriveLane();
};
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index e4621b82bb63e5ece82f628977c87cd9ecae6b3d..ddda090bae8d56c1654a5e53b98b0a50a3532c08 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -511,16 +511,14 @@ void COpendriveLane::load(const ::lane &lane_info,COpendriveRoadSegment *segment
this->set_width(lane_info.width().begin()->a().get());
// import lane speed
if(lane_info.speed().size()<1)
- {
- error << "Lane " << this->id << " of segment " << segment->get_name() << " has no speed record";
- throw CException(_HERE_,error.str());
- }
+ this->set_speed(60.0);
else if(lane_info.speed().size()>1)
{
error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one speed record";
throw CException(_HERE_,error.str());
}
- this->set_speed(lane_info.speed().begin()->max().get());
+ else
+ this->set_speed(lane_info.speed().begin()->max().get());
//lane mark
if(lane_info.roadMark().size()==0)
mark=OD_MARK_NONE;