Commit 7534e38d authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a function to get the previous segment.

Made some functions const.
parent 51fec26a
......@@ -63,17 +63,15 @@ class COpendriveRoadSegment
void add_node(TOpendriveRoadSegmentGeometry &geometry,COpendriveLane *lane);
void add_empty_node(TOpendriveWorldPose &pose,COpendriveLane *lane);
void remove_node(COpendriveRoadNode *node);
bool has_node(COpendriveRoadNode *node);
int get_node_side(COpendriveRoadNode *node);
bool has_node(COpendriveRoadNode *node) const;
void link_neightbour_lanes(lanes::laneSection_type &lane_section);
void link_neightbour_lanes(void);
void link_segment(COpendriveRoadSegment *segment);
void link_segment(COpendriveRoadSegment *segment,int from,bool from_start, int to,bool to_start);
bool connects_to(COpendriveRoadSegment *segment);
bool connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2);
COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL);
COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref);
COpendriveRoadSegment *get_next_segment(int &side);
COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL) const;
COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref) const;
public:
std::string get_name(void) const;
unsigned int get_id(void) const;
......@@ -89,16 +87,19 @@ class COpendriveRoadSegment
const COpendriveObject &get_object(unsigned int index) const;
unsigned int get_num_connecting(void) const;
const COpendriveRoadSegment &get_connecting(unsigned int index) const;
double get_length(void);
TOpendriveWorldPose get_pose_at(double length);
double get_curvature_at(double length);
double get_length(void) const;
TOpendriveWorldPose get_pose_at(double length) const;
double get_curvature_at(double length) const;
TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track);
TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track);
const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1);
const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
int get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
const COpendriveRoadSegment &get_next_segment(int &side,bool &valid) const;
const COpendriveRoadSegment &get_prev_segment(int &side,bool &valid) const;
int get_pose_side(TOpendriveWorldPose &pose) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
~COpendriveRoadSegment();
};
......
......@@ -521,7 +521,7 @@ void COpendriveRoadSegment::remove_node(COpendriveRoadNode *node)
}
}
bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node)
bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node) const
{
for(unsigned int i=0;i<this->nodes.size();i++)
if(this->nodes[i]==node)
......@@ -530,25 +530,20 @@ bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node)
return false;
}
int COpendriveRoadSegment::get_node_side(COpendriveRoadNode *node)
int COpendriveRoadSegment::get_pose_side(TOpendriveWorldPose &pose) const
{
std::map<int,COpendriveLane *>::iterator it;
const COpendriveRoadNode *node;
std::map<int,COpendriveLane *>::const_iterator it;
std::stringstream error;
double distance;
if(this->has_node(node))
{
for(it=this->lanes.begin();it!=this->lanes.end();it++)
if(it->second->has_node(node))
return it->first;
node=&this->get_closest_node(pose,distance,3.14159);
for(it=this->lanes.begin();it!=this->lanes.end();it++)
if(it->second->has_node((COpendriveRoadNode *)node))
return it->first;
error << "Segment " << this->name << " does not include node " << node->get_index();
throw CException(_HERE_,error.str());
}
else
{
error << "Segment " << this->name << " does not include node " << node->get_index();
throw CException(_HERE_,error.str());
}
error << "Segment " << this->name << " does not include node " << node->get_index();
throw CException(_HERE_,error.str());
}
void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
......@@ -690,7 +685,7 @@ bool COpendriveRoadSegment::connects_segments(COpendriveRoadSegment *segment1,CO
return false;
}
COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start,TOpendriveWorldPose *end) const
{
TOpendriveWorldPose *start_pose,*end_pose;
std::map<int,COpendriveLane *>::iterator lane_it;
......@@ -708,7 +703,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
if(start==NULL && end==NULL)
return this->clone(new_node_ref,new_lane_ref,new_link_ref);
new_segment=new COpendriveRoadSegment(*this);
new_segment_ref[this]=new_segment;
new_segment_ref[(COpendriveRoadSegment *)this]=new_segment;
if(node_side<0)
{
start_pose=start;
......@@ -770,7 +765,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
return new_segment;
}
COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref)
COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref) const
{
std::map<int,COpendriveLane *>::iterator lane_it;
lane_up_ref_t::iterator lane_ref_it;
......@@ -781,7 +776,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
node_up_ref_t::iterator node_ref_it;
new_segment=new COpendriveRoadSegment(*this);
new_segment_ref[this]=new_segment;
new_segment_ref[(COpendriveRoadSegment *)this]=new_segment;
/* get the sublanes */
for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
{
......@@ -793,7 +788,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
return new_segment;
}
COpendriveRoadSegment *COpendriveRoadSegment::get_next_segment(int &side)
const COpendriveRoadSegment &COpendriveRoadSegment::get_next_segment(int &side,bool &valid) const
{
std::vector<COpendriveRoadSegment *> segment_candidates;
std::vector<int> side_candidates;
......@@ -848,16 +843,90 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_next_segment(int &side)
}
}
if(segment_candidates.size()==0)
return NULL;
{
valid=false;
return *this;
}
else if(segment_candidates.size()>1)
throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
else
{
valid=true;
side=side_candidates[0];
return *segment_candidates[0];
}
}
const COpendriveRoadSegment &COpendriveRoadSegment::get_prev_segment(int &side,bool &valid) const
{
std::vector<COpendriveRoadSegment *> segment_candidates;
std::vector<int> side_candidates;
bool already_present;
if(side<0)
{
for(unsigned int i=1;i<=this->get_num_right_lanes();i++)
{
const COpendriveLane &lane=this->get_lane(-i);
if(lane.get_num_prev_lanes()>1)
throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
if(lane.get_num_prev_lanes()==1)
{
already_present=false;
for(unsigned int k=0;k<segment_candidates.size();k++)
if(segment_candidates[k]==lane.get_prev_lane(0).segment)
{
already_present=true;
break;
}
if(!already_present)
{
segment_candidates.push_back(lane.get_prev_lane(0).segment);
side_candidates.push_back(lane.get_prev_lane(0).get_id());
}
}
}
}
else
{
for(unsigned int i=1;i<=this->get_num_left_lanes();i++)
{
const COpendriveLane &lane=this->get_lane(i);
if(lane.get_num_prev_lanes()>1)
throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
if(lane.get_num_prev_lanes()==1)
{
already_present=false;
for(unsigned int k=0;k<segment_candidates.size();k++)
if(segment_candidates[k]==lane.get_prev_lane(0).segment)
{
already_present=true;
break;
}
if(!already_present)
{
segment_candidates.push_back(lane.get_prev_lane(0).segment);
side_candidates.push_back(lane.get_prev_lane(0).get_id());
}
}
}
}
if(segment_candidates.size()==0)
{
valid=false;
return *this;
}
else if(segment_candidates.size()>1)
throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
else
{
valid=true;
side=side_candidates[0];
return segment_candidates[0];
return *segment_candidates[0];
}
}
void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
{
std::map<int,COpendriveLane *>::const_iterator lane_it;
......@@ -1034,17 +1103,17 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int
}
}
double COpendriveRoadSegment::get_length(void)
double COpendriveRoadSegment::get_length(void) const
{
double length=0.0;
for(unsigned int i=0;i<this->geometries.size();i++)
length+=this->geometries[i].opendrive->get_length();
length+=this->geometries[i].spline->get_length();
return length;
}
TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length)
TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length) const
{
TOpendriveWorldPose world_pose{0};
TPoint spline_pose;
......@@ -1066,7 +1135,7 @@ TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length)
return world_pose;
}
double COpendriveRoadSegment::get_curvature_at(double length)
double COpendriveRoadSegment::get_curvature_at(double length) const
{
double curvature;
TPoint spline_pose;
......@@ -1207,12 +1276,18 @@ int COpendriveRoadSegment::get_closest_lane_id(TOpendriveWorldPose &pose,double
return closest_id;
}
const COpendriveLane &COpendriveRoadSegment::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol)
const COpendriveLane &COpendriveRoadSegment::get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
std::map<int,COpendriveLane *>::const_iterator lane_it;
int closest_id=this->get_closest_lane_id(pose,distance,angle_tol);
if(closest_id==0)
throw CException(_HERE_,"Impossible to find the closest lane");
return *this->lanes[closest_id];
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
if(lane_it->first==closest_id)
return *lane_it->second;
throw CException(_HERE_,"Impossible to find the closest lane");
}
double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
......@@ -1246,7 +1321,6 @@ double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendr
{
for(unsigned int i=0;i<closest_index;i++)
distance+=this->geometries[i].spline->get_length();
distance+=min_dist;
}
else
distance=std::numeric_limits<double>::max();
......
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