Commit 0477fd07 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Updated the road node documentation.

parent 5b6a3508
doc/images/ref_system_road_nodes.png

157 KB | W: | H:

doc/images/ref_system_road_nodes.png

165 KB | W: | H:

doc/images/ref_system_road_nodes.png
doc/images/ref_system_road_nodes.png
doc/images/ref_system_road_nodes.png
doc/images/ref_system_road_nodes.png
  • 2-up
  • Swipe
  • Onion skin
......@@ -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();
};
......
......@@ -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);
}
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment