diff --git a/doc/opendrive_link.md b/doc/opendrive_link.md
index 63d7f14c69a1ff2a3bce10cd1c9e40dd1bf38567..486ad4708eaae3f6a1ae9efaa923c121cfe9cdb1 100644
--- a/doc/opendrive_link.md
+++ b/doc/opendrive_link.md
@@ -3,29 +3,40 @@ OpendriveLink
## Description
-This class describes the connection between two road nodes. This connection is defined in a single direction which coincides with the traffic direction.
+This class describes the connection between two [COpendriveRoadNode](opendrive_road_node.md) road nodes. This connection is defined in a single direction which coincides with the traffic direction.
-The next image shows (as solid arrows) the set of COpendriveLink's defined for the example road presented in [here]().
+The next image shows (as solid arrows) the set of COpendriveLink's defined for the example road presented in [here](), with the COpendriveRoadNode's objects (as red dots).
-<img src="images/links.png" alt="COpendriveRoadNode objects created for the example road" width="607" height="544">
+<img src="images/links.png" alt="COpendriveLink objects created for the example road" width="607" height="544">
-This class encodes the geometry as [G2 Spline]().
+This class describes the geometry as [G2 Spline](g2_spline.md).
Links are created between a road node and:
* the next road node on the same lane and road segment (if any).
* the previous road node on the same lane and road segment (if any).
+* the first or last road node on connecting lanes and road segments.
+
+These type of links (called **lane links**) describe the actual geometry of the road and belong to a specific lane and road segment. Additional, links are also created between a road node and:
+
* the next road node on the adjacent lane (left or right) and the same road segment (if any).
* the previous road node on the adjacent lane (left or right) and the same road segment (if any).
-* the first or last road node on connecting lanes and road segment.
+
+This second type of links (called **non-lane links**) do not belong to any specific lane, only to a road segment.
+
+In road junctions, the only links that are created are the ones defined by the Opendrive junction connectivity.
## Main features
The main features of this class are:
-* Provides the reference to the parent lane [COpendriveLane]() and parent road segment [COpendriveRoadSegment]().
-* Provides the reference to the starting and ending COpendriveRoadNode objects.
-* It is possible to generate the actual path defined by the internal geometry.
-* It is possible to find the closest position to the link geometry.
+* Provides the reference to the starting and ending [COpendriveRoadNode](opendrive_road_node.md) objects.
+* Provides road mark information between its two road nodes. For lane links this will always be OD_MARK_NONE, but for non-lane links it will correspond to the actual road mark.
+* Provides the reference to the parent lane [COpendriveLane](opendrive_lane.md) only for lane links
+* Provides the reference to the parent road segment [COpendriveRoadSegment](opendrive_road_segment.md).
+* It is possible to find the closest pose (minimum distance) on the link geometry to a given poition.
+* It is possible to generate the actual trajectory (sub-trajectory) defined by the internal geometry (x and y coordinates and optionally the heading).
+* It is possible to get the pose and curvature at any point on the link 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.
\ No newline at end of file
diff --git a/include/opendrive_link.h b/include/opendrive_link.h
index 74500b0f50993f6fed37f7b2b5214991dde38a87..675be45474866b9f315b31e789b6ee7a7559717e 100644
--- a/include/opendrive_link.h
+++ b/include/opendrive_link.h
@@ -34,25 +34,188 @@ class COpendriveLink
void set_resolution(double res);
void set_scale_factor(double scale);
void generate(double start_curvature,double end_curvature);
- void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lanei_refs,node_up_ref_t &node_refs);
+ void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
bool clean_references(node_up_ref_t &refs);
void update_start_pose(TOpendriveWorldPose &start,double curvature);
void update_end_pose(TOpendriveWorldPose &end,double curvature);
public:
+ /**
+ * \brief Returns a reference to the previous road node
+ *
+ * This function returns a constant reference to the previous COpendriveRoadNode
+ * object. If the previous road node is not defines, an exception will be thrown.
+ *
+ * \return a constant reference to the previous COpendriveRoadNode object.
+ */
const COpendriveRoadNode &get_prev(void) const;
+ /**
+ * \brief Returns a reference to the next road node
+ *
+ * This function returns a constant reference to the next COpendriveRoadNode
+ * object. If the next road node is not defines, an exception will be thrown.
+ *
+ * \return a constant reference to the next COpendriveRoadNode object.
+ */
const COpendriveRoadNode &get_next(void) const;
+ /**
+ * \brief Returns the road mark identifier between the two road nodes
+ *
+ * This function returns the possible road mark between the two COpendriveRoadNode
+ * objects linked by each instance of this class. For lane links this will always
+ * be OD_MARK_NONE, but for non-lane links it will correspond to the actual road
+ * mark.
+ *
+ * \return the road mark identifier.
+ */
opendrive_mark_t get_road_mark(void) const;
+ /**
+ * \brief Returns the reference to the parent road segment
+ *
+ * This function returns a constant reference to the COpendriveRoadSegment object
+ * where the each instance of this class belongs. If the reference is not valid,
+ * an exception will be thrown.
+ *
+ * \return a constant reference to the parent COpendriveRoadSegment object.
+ */
const COpendriveRoadSegment &get_parent_segment(void) const;
+ /**
+ * \brief Returns the reference to the parent lane
+ *
+ * This function returns a constant reference to the COpendriveLane object
+ * where the each instance of this class belongs. This function will fail for
+ * non-lane links since they do not have a parent lane. In this case an
+ * exception will be thrown.
+ *
+ * Use the is_lane_link() function to check the type of link.
+ *
+ * \return a constant reference to the parent COpendriveRoadSegment object.
+ */
const COpendriveLane &get_parent_lane(void) const;
+ /**
+ * \brief returns the type of link
+ *
+ * This function returns whether each instance of this class is a lane link
+ * or not.
+ *
+ * \return true if the instance is a lane link and false otherwise.
+ */
+ bool is_lane_link(void) const;
+ /**
+ * \brief Returns the resolution
+ *
+ * \return This function returns the current resolution
+ */
double get_resolution(void) const;
+ /**
+ * \brief Returns the scale factor
+ *
+ * \return This function returns the current scale factor
+ */
double get_scale_factor(void) const;
- double find_closest_world_pose(TOpendriveWorldPose &world,TOpendriveWorldPose &pose);
+ /**
+ * \brief returns the closest pose to a given one
+ *
+ * This functions finds the closest pose (minimum distance) on the link geometry
+ * to a given position. The closest pose only exist if the imaginary line between the
+ * given pose and its projection into the link is orthogonal to the link geometry.
+ * Otherwise, it is considered that the given pose does not coincide with the link
+ * object.
+ *
+ * \param world given pose to find the closest point. The heading information is
+ * ignored.
+ * \param pose the closest pose on the link geometry if the given pose coincide
+ * with the link object. Otherwise the data on this parameters is not
+ * valid.
+ *
+ * \return the distance between the projected pose and the link start position if
+ * given pose coincide with the link object. Otherwise the maximum double
+ * value (std::numeric_limits<double>::max()) will be returned.
+ */
+ double find_closest_world_pose(TOpendriveWorldPose &world,TOpendriveWorldPose &pose) const;
+ /**
+ * \brief returns the link trajectory (only position)
+ *
+ * This function returns the x and y coordinates of the link geometry evaluated at
+ * the internal resolution. By default the whole trajectory is returned, but is
+ * possible to get only a sub-trajectory by specifying the start and end lengths.
+ *
+ * \param x a vector with the x coordinates of the link trajectory (sub-trajectory)
+ * in the world reference system.
+ * \param y a vector with the y coordinates of the link trajectory (sub-trajectory)
+ * in the world reference system.
+ * \param start_length initial distance from the origin position of the link in
+ * meters. By default this value is 0.0.
+ * \param end_length final distance from the origin position of the link in meters.
+ * By default this value is -1.0 to indicate the end of the link.
+ */
void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;
+ /**
+ * \brief returns the link trajectory (position and heading)
+ *
+ * This function returns the x and y coordinates, as well as the heading of the
+ * link geometry evaluated at the internal resolution. By default the whole
+ * trajectory is returned, but is possible to get only a sub-trajectory by
+ * specifying the start and end lengths.
+ *
+ * \param x a vector with the x coordinates of the link trajectory (sub-trajectory)
+ * in the world reference system.
+ * \param y a vector with the y coordinates of the link trajectory (sub-trajectory)
+ * in the world reference system.
+ * \param yaw a vector with the heading of the link trajectory (sub-trajectory)
+ * in the world reference system.
+ * \param start_length initial distance from the origin position of the link in
+ * meters. By default this value is 0.0.
+ * \param end_length final distance from the origin position of the link in meters.
+ * By default this value is -1.0 to indicate the end of the link.
+ */
void get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length=0.0, double end_length=-1.0) const;
+ /**
+ * \brief returns the length of the link
+ *
+ * This function returns the length of the link geometry.
+ *
+ * \return length of the link geometry in meters.
+ */
double get_length(void) const;
+ /**
+ * \brief returns the pose of the link at a given point
+ *
+ * This function returns the pose (x and y coordinates and heading) of the link geometry
+ * at a given distance from the origin point of the link.
+ *
+ * \param length desired distance from the origin point of the link in meters. This
+ * distance must be smaller or equal than the actual link length.
+ *
+ * \return the pose at the desired point in the world reference system.
+ */
TOpendriveWorldPose get_pose_at(double length);
+ /**
+ * \brief returns the curvature of the link at a given point
+ *
+ * This function returns the curvature of the link geometry at a given distance from
+ * the origin point of the link.
+ *
+ * \param length desired distance from the origin point of the link in meters. This
+ * distance must be smaller or equal than the actual link length.
+ *
+ * \return the curvature at the desired point.
+ */
double get_curvature_at(double length);
+ /**
+ * \brief overloaded stream operator
+ *
+ * This functions streams human readable information about the link. This information
+ * includes:
+ * * Index of the previous and next COpendriveRoad node objects.
+ * * Name of the parent COpendriveROadSegment object.
+ * * Id of the parent lane for lane links.
+ * * The road mark between the two road nodes.
+ *
+ */
friend std::ostream& operator<<(std::ostream& out, COpendriveLink &link);
+ /**
+ * \brief Destructor
+ */
~COpendriveLink();
};
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 782bb0b570cd7bbd44c75262c7b09a23540d98fc..89ecedefcee6bd4b28672c56cf534f93a86d62d8 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -158,12 +158,18 @@ void COpendriveLink::update_end_pose(TOpendriveWorldPose &end,double curvature)
const COpendriveRoadNode &COpendriveLink::get_prev(void) const
{
- return *this->prev;
+ if(this->prev!=NULL)
+ return *this->prev;
+
+ throw CException(_HERE_,"Invalid previous road node reference. Link is not properly defined");
}
const COpendriveRoadNode &COpendriveLink::get_next(void) const
{
- return *this->next;
+ if(this->next!=NULL)
+ return *this->next;
+
+ throw CException(_HERE_,"Invalid next road node reference. Link is not properly defined");
}
opendrive_mark_t COpendriveLink::get_road_mark(void) const
@@ -173,7 +179,10 @@ opendrive_mark_t COpendriveLink::get_road_mark(void) const
const COpendriveRoadSegment &COpendriveLink::get_parent_segment(void) const
{
- return *this->segment;
+ if(this->segment!=NULL)
+ return *this->segment;
+
+ throw CException(_HERE_,"Invalid parent road segment reference. Link is not properky defined");
}
const COpendriveLane &COpendriveLink::get_parent_lane(void) const
@@ -189,6 +198,14 @@ const COpendriveLane &COpendriveLink::get_parent_lane(void) const
}
}
+bool COpendriveLink::is_lane_link(void)
+{
+ if(this->lane==NULL)
+ return false;
+ else
+ return true;
+}
+
double COpendriveLink::get_resolution(void) const
{
return this->resolution;
@@ -199,7 +216,7 @@ double COpendriveLink::get_scale_factor(void) const
return this->scale_factor;
}
-double COpendriveLink::find_closest_world_pose(TOpendriveWorldPose &world,TOpendriveWorldPose &pose)
+double COpendriveLink::find_closest_world_pose(TOpendriveWorldPose &world,TOpendriveWorldPose &pose) const
{
TPoint spline_pose;
double length;