diff --git a/doc/images/ref_system_road_nodes.png b/doc/images/ref_system_road_nodes.png
index abbc7a5b5234bb4d879a12f25244bd8c2da18856..13113460031a85e17459c6fe83bfa7da3d342398 100644
Binary files a/doc/images/ref_system_road_nodes.png and b/doc/images/ref_system_road_nodes.png differ
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 1436a113df687f98954ee859b7b3b118457c5351..f2200a63de999e89a7b70e471b6e8c915ed0b742 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -51,22 +51,252 @@ class COpendriveRoadNode
void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance);
void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance);
public:
+ /**
+ * \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 node index
+ *
+ * This function returns the unique node index which identified the road node object.
+ * This index is assigned when the Opendrive file is loaded and may only change when
+ * a new sub-road is generated, in which case the road nodes in the new road are re-
+ * indexed.
+ *
+ * \return the index of the road node.
+ */
unsigned int get_index(void) const;
+ /**
+ * \brief Returns the number of parents
+ *
+ * This function returns how many parents the road node have. Each parent is identified
+ * by the road segment and also the lane where it belongs.
+ *
+ * \return This function returns the number of parent of the road node. The range of
+ * valid indexes goes from 0 to the value returned by this function -1.
+ */
unsigned int get_num_parents(void) const;
+ /**
+ * \brief Returns a reference to a road segment parent
+ *
+ * This function returns a constant reference to a COpendriveRoadSegment object which
+ * is a parent of the road node. This reference 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 parent
+ *
+ * \return constant reference to the desired road segment parent object.
+ */
const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
+ /**
+ * \brief Returns a reference to a lane parent
+ *
+ * This function returns a constant reference to a COpendriveLane object which
+ * is a parent of the road node. This reference 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 parent
+ *
+ * \return constant reference to the desired lane parent object.
+ */
const COpendriveLane &get_parent_lane(unsigned int index) const;
+ /**
+ * \brief returns the position of the road node
+ *
+ * This function returns the pose of the road node in the world reference system.
+ *
+ * \return world pose of the road node.
+ */
TOpendriveWorldPose get_pose(void) const;
+ /**
+ * \brief Returns the number of links
+ *
+ * This function returns how many links are connecting to this road node, either
+ * starting or ending at it.
+ *
+ * \return This function returns the number of links of the road node. The range of
+ * valid indexes goes from 0 to the value returned by this function -1.
+ */
unsigned int get_num_links(void) const;
+ /**
+ * \brief Returns a reference to a link
+ *
+ * This function returns a constant reference to a COpendriveLink object which
+ * connects this node to another one. In this case both links starting and ending
+ * at the desired road node can be accessed. This reference 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 parent
+ *
+ * \return constant reference to the desired link object.
+ */
const COpendriveLink &get_link(unsigned int index) const;
+ /**
+ * \brief Returns a reference to the link connecting with the given road node
+ *
+ * This function returns a constant reference to the COpendriveLink object that
+ * connects this road node with another one, identified by its unique index. If
+ * there exist no connection between the two road nodes, an exception is thrown.
+ *
+ * In this case, only links starting at the road node calling this function can
+ * be accessed.
+ *
+ * \param node_index unique identifier of the road node candidate to check for
+ * connectivity.
+ *
+ * \return constant reference to the desired link object, it it exists.
+ */
const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
+ /**
+ * \brief Returns a reference to the link connecting with the given road node
+ *
+ * This function returns a constant reference to the COpendriveLink object that
+ * connects this road node with another one. If there exist no connection between
+ * the two road nodes, an exception is thrown.
+ *
+ * In this case, only links starting at the road node calling this function can
+ * be accessed.
+ *
+ * \param node COpendriveRoadNode object candidate to check for connectivity.
+ *
+ * \return constant reference to the desired link object, it it exists.
+ */
+ const COpendriveLink &get_link_ending_at(const COpendriveRoadNode &node) const;
+ /**
+ * \brief checks if a link with another road node exists
+ *
+ * This function checks if a road node is connected through a link with another
+ * road node, identified by its unique index.
+ *
+ * \param node_index unique identifier of the road node candidate to check for
+ * connectivity.
+ *
+ * \return true if the two road nodes are connected, or false otherwise.
+ */
+ bool has_link_with(unsigned int node_index) const;
+ /**
+ * \brief checks if a link with another road node exists
+ *
+ * This function checks if a road node is connected through a link with another
+ * road node.
+ *
+ * \param node COpendriveRoadNode object candidate to check for connectivity.
+ *
+ * \return true if the two road nodes are connected, or false otherwise.
+ */
bool has_link_with(const COpendriveRoadNode &node) const;
- unsigned int get_closest_link(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
- double get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes=false,double angle_tol=0.1) const;
+ /**
+ * \brief returns the index of the closest link to a given pose
+ *
+ * This functions finds the closest link starting on this road node to a given
+ * pose, and returns its index. This function only returns a valid index
+ * when the given pose coincides with the road node links.
+ *
+ * \param world given pose to find the closest link.
+ * \param distance the link distance between the projection of the given pose to
+ * the closest link geometry, and the position of the starting road node,
+ * following the closest link geometry. If the closest link does not exist
+ * this argument is set to the maximum double value
+ * (std::numeric_limits<double>::max())
+ * \only_lanes flag to indicate it the search for the closest link should include
+ * only the lane links (true) or all of them (false).
+ * \angle_tol maximum heading difference between the given pose and the closest pose
+ * on the link geometry to consider each link as a candidate on the search.
+ *
+ * \return the index of the closest link or -1 if there is no link close to the given
+ * pose.
+ */
+ unsigned int get_closest_link_index(TOpendriveWorldPose &pose,double &distance,bool only_lanes=false,double angle_tol=0.1) const;
+ /**
+ * \brief returns the closest link to a given pose
+ *
+ * This functions finds the closest link starting on this road node to a given
+ * pose, and returns it. This function only returns a valid reference when the
+ * given pose coincides with the road node links. Otherwise, it throws an exception.
+ *
+ * \param world given pose to find the closest link.
+ * \param distance the link distance between the projection of the given pose to
+ * the closest link geometry, and the position of the starting road node,
+ * following the closest link geometry. If the closest link does not exist
+ * this argument is set to the maximum double value
+ * (std::numeric_limits<double>::max())
+ * \only_lanes flag to indicate it the search for the closest link should include
+ * only the lane links (true) or all of them (false).
+ * \angle_tol maximum heading difference between the given pose and the closest pose
+ * on the link geometry to consider any link as a candidate on the search.
+ *
+ * \return a constant reference to the closest link which can not be modified by the
+ * user.
+ */
+ const COpendriveLink &get_closest_link(TOpendriveWorldPose &pose,double &distance,bool only_lanes=false,double angle_tol=0.1) const;
+ /**
+ * \brief returns the closest pose at any link to a given pose
+ *
+ * This functions finds the closest pose (minimum distance) on any link geometry
+ * to a given pose. The closest pose only exist if the imaginary line between the
+ * given pose and its projection into any link is orthogonal to the corresponding
+ * link geometry. Otherwise, it is considered that the given pose does not coincide
+ * with the road node links.
+ *
+ * \param world given pose to find the closest link.
+ * \param closest_pose the closest pose to the closest link geometry if the given pose
+ * coincides with the link object. Otherwise the data on this
+ * parameters is not valid.
+ * \only_lanes flag to indicate it the search for the closest link should include
+ * only the lane links (true) or all of them (false).
+ * \angle_tol maximum heading difference between the given pose and the closest pose
+ * on the link geometry to consider any link as a candidate on the search.
+ *
+ * \return the link distance between the projection of the given pose to the
+ * closest link geometry, following its geometry. If there is no
+ * closest link, the maximum double value (std::numeric_limits<double>::max())
+ * will be returned.
+ */
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes=false,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,bool only_lanes=false,double angle_tol=0.1) const;
+ /**
+ * \brief Returns a set of poses on any link close to a given pose
+ *
+ * This function is similar to the get_closest_pose(), but in this case, the closest
+ * pose of any link which is closer to the given pose that a given distance will be
+ * returned.
+ *
+ * \param world given pose to find the closest link.
+ * \param closest_poses the closest poses on any link geometry if the given pose
+ * coincide with the link object. Otherwise an empty vector will
+ * be returned.
+ * \param distances the link distance between the projection of the given pose to each
+ * of the closest link geometries, following the corresponding geometry.
+ * If the given pose does not coincide with any of the links, an empty
+ * vector will be returned.
+ * \only_lanes flag to indicate it the search for the closest link should include
+ * only the lane links (true) or all of them (false).
+ * \angle_tol maximum heading difference between the given pose and the closest pose
+ * on the link geometry to consider any link as a candidate on the search.
+ */
+ void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &distances,double dist_tol,bool only_lanes=false,double angle_tol=0.1) const;
+ /**
+ * \brief overloaded stream operator
+ *
+ * This functions streams human readable information about the road node. This information
+ * includes:
+ * * The node index.
+ * * Its position and heading.
+ * * for each parent: the name of the segment and the lane id.
+ * * The links information.
+ *
+ */
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
+ /**
+ * \brief Destructor
+ */
~COpendriveRoadNode();
};
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index bff1134db2d95040609a762c56e1bd516b6b48b1..0a87c10b464c8c6896430982fa119a658140b75c 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -127,6 +127,15 @@ bool COpendriveRoadNode::has_link_with(const COpendriveRoadNode &node) const
return false;
}
+bool COpendriveRoadNode::has_link_with(unsigned int node_index) const
+{
+ for(unsigned int i=0;i<this->links.size();i++)
+ if(this->links[i]->prev==this && this->links[i]->next->get_index()==node_index)
+ return true;
+
+ return false;
+}
+
COpendriveLink *COpendriveRoadNode::get_link_with(COpendriveRoadNode *node)
{
for(unsigned int i=0;i<this->links.size();i++)
@@ -426,45 +435,29 @@ const COpendriveLink &COpendriveRoadNode::get_link_ending_at(unsigned int node_i
return *this->links[i];
}
- text << "No link in node " << this->index << " ends at node " << node_index;;
+ text << "No link in node " << this->index << " ends at node " << node_index;
throw CException(_HERE_,text.str());
}
-unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,double angle_tol)const
+const COpendriveLink &COpendriveRoadNode::get_link_ending_at(const COpendriveRoadNode &node) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
- unsigned int closest_index=-1;
- double angle,length;
- TOpendriveWorldPose closest_pose;
+ std::stringstream text;
for(unsigned int i=0;i<this->links.size();i++)
{
- if(&this->links[i]->get_prev()==this)// links starting at this node
- {
- length=this->links[i]->find_closest_world_pose(pose,closest_pose);
- if(length<std::numeric_limits<double>::max())
- {
- angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
- if(fabs(angle)<angle_tol)
- {
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
- {
- min_dist=dist;
- closest_index=i;
- }
- }
- }
- }
+ if(&this->links[i]->get_next()==&node)
+ return *this->links[i];
}
- return closest_index;
+ text << "No link in node " << this->index << " ends at node " << node.get_index();
+ throw CException(_HERE_,text.str());
}
-double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes,double angle_tol)const
+unsigned int COpendriveRoadNode::get_closest_link_index(TOpendriveWorldPose &pose,double &distance,bool only_lanes,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
- double angle,length;
+ unsigned int closest_index=-1;
+ double angle,length,min_length=std::numeric_limits<double>::max();
TOpendriveWorldPose closest_pose;
for(unsigned int i=0;i<this->links.size();i++)
@@ -481,14 +474,29 @@ double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,bool o
{
dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
if(dist<min_dist)
+ {
min_dist=dist;
+ min_length=length;
+ closest_index=i;
+ }
}
}
}
}
}
- return min_dist;
+ distance=min_length;
+ return closest_index;
+}
+
+const CopendriveLink &COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,double &distance,bool only_lanes,double angle_tol)const
+{
+ unsigned int closest_index;
+
+ closest_index=this->get_closest_link_index(pose,distance,only_lanes,angle_tol);
+ if(closest_index==(unsigned int)-1)
+ throw CException(_HERE_,"Impossible to find the closest link");
+ return *this->links[closest_index];
}
double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes,double angle_tol) const
@@ -529,7 +537,7 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
return closest_length;
}
-void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes,double angle_tol) const
+void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &distances,double dist_tol,bool only_lanes,double angle_tol) const
{
double dist;
double angle;
@@ -538,7 +546,7 @@ void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector
bool already_added;
closest_poses.clear();
- closest_lengths.clear();
+ distances.clear();
for(unsigned int i=0;i<this->links.size();i++)
{
if(&this->links[i]->get_prev()==this)// links starting at this node
@@ -565,7 +573,7 @@ void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector
if(!already_added)
{
closest_poses.push_back(closest_pose);
- closest_lengths.push_back(length);
+ distances.push_back(length);
}
}
}