diff --git a/src/examples/opendrive_test.cpp b/src/examples/opendrive_test.cpp
index 566ed4cb1a7feece4f7a6690c3bc4db8c34c702c..b5f6d5c7a2faf3d43091e45960b41ece9eebf748 100644
--- a/src/examples/opendrive_test.cpp
+++ b/src/examples/opendrive_test.cpp
@@ -16,7 +16,7 @@ int main(int argc, char *argv[])
// std::string xml_file="/home/shernand/Downloads/osm2xodr_old/output.xodr";
// std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/add.xodr";
std::string xml_file="/home/shernand/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/build/new_road.xodr";
-// std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr";
+// std::string xml_file="/home/shernand/iri-lab/adc_ws/code/autonomous_driving_tools/src/xml/adc.xodr";
// std::string xml_file="/home/shernand/iri-lab/iri_team_ws/src/iri_adc_launch/data/adc/adc.xodr";
try
@@ -25,10 +25,19 @@ int main(int argc, char *argv[])
road.set_scale_factor(1.0);
road.set_min_road_length(0.1);
road.load(xml_file);
-// return 0;
// std::cout << road << std::endl;
// return 0;
+/*
+ for(unsigned int i=0;i<road.get_num_nodes();i++)
+ {
+ const COpendriveRoadNode &node=road.get_node(i);
+ TOpendriveWorldPose pose=node.get_pose();
+ std::cout << pose.x << "," << pose.y << "," << pose.heading << std::endl;
+ }
+
+ return 0;
+*/
for(unsigned int i=0;i<road.get_num_nodes();i++)
{
const COpendriveRoadNode &node=road.get_node(i);
@@ -65,7 +74,7 @@ int main(int argc, char *argv[])
start_pose.x=52.2;
start_pose.y=15.0;
start_pose.heading=0.0;
- new_path=road.get_sub_road(path,end_pose,new_road,false);
+ new_path=road.get_sub_road(path,end_pose,new_road);
new_path2=new_road.get_sub_road(start_pose,end_pose,length,new_road2,new_path,-1);
for(unsigned int i=0;i<new_road2.get_num_nodes();i++)
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 0f6b535053f9d408ef403c1378c6b54be16a039f..ee78ab6006c11ee8576dfbe7f0b1e6da2e84cba5 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -57,7 +57,10 @@ COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object
this->geometries[i].spline=new CG2Spline(*object.geometries[i].spline);
}
this->junction_segment=object.junction_segment;
- this->junction=object.junction;
+ if(object.junction!=NULL)
+ this->junction=new COpendriveJunction(*object.junction);
+ else
+ this->junction=NULL;
}
COpendriveRoadSegment::COpendriveRoadSegment(const std::string &name,opendrive_mark_t center_mark,unsigned int id,bool junction_segment)
@@ -89,9 +92,10 @@ COpendriveRoadSegment::COpendriveRoadSegment(const std::string &name,opendrive_m
this->scale_factor=DEFAULT_SCALE_FACTOR;
this->geometries.clear();
this->junction_segment=junction_segment;
+ this->junction=NULL;
}
-void COpendriveRoadSegment::add_left_lane(COpendriveLane &lane)
+int COpendriveRoadSegment::add_left_lane(COpendriveLane &lane)
{
COpendriveLane *new_lane;
@@ -102,6 +106,7 @@ void COpendriveRoadSegment::add_left_lane(COpendriveLane &lane)
this->num_left_lanes++;
this->lanes[this->num_left_lanes]=new_lane;
new_lane->set_id(this->num_left_lanes);
+ return this->num_left_lanes;
}catch(CException &e){
this->free();
delete new_lane;
@@ -109,7 +114,7 @@ void COpendriveRoadSegment::add_left_lane(COpendriveLane &lane)
}
}
-void COpendriveRoadSegment::add_right_lane(COpendriveLane &lane)
+int COpendriveRoadSegment::add_right_lane(COpendriveLane &lane)
{
COpendriveLane *new_lane;
@@ -120,6 +125,7 @@ void COpendriveRoadSegment::add_right_lane(COpendriveLane &lane)
this->num_right_lanes++;
this->lanes[-this->num_right_lanes]=new_lane;
new_lane->set_id(-this->num_right_lanes);
+ return -this->num_right_lanes;
}catch(CException &e){
this->free();
delete new_lane;
@@ -383,17 +389,17 @@ void COpendriveRoadSegment::update_references(segment_up_ref_t &segment_refs,lan
std::map<int,COpendriveLane *>::iterator lane_it;
std::vector<COpendriveRoadNode *>::iterator node_it;
- if(this->updated(segment_refs))
- {
+// if(this->updated(segment_refs))
+// {
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
{
if(lane_refs.find((*lane_it).second)!=lane_refs.end())
{
lane_it->second=lane_refs[lane_it->second];
- lane_it->second->update_references(segment_refs,lane_refs,node_refs);
+// lane_it->second->update_references(segment_refs,lane_refs,node_refs);
}
- else if(lane_it->second->updated(lane_refs))
- lane_it->second->update_references(segment_refs,lane_refs,node_refs);
+// else if(lane_it->second->updated(lane_refs))
+// lane_it->second->update_references(segment_refs,lane_refs,node_refs);
}
for(node_it=this->nodes.begin();node_it!=this->nodes.end();node_it++)
if(node_refs.find(*node_it)!=node_refs.end())
@@ -409,22 +415,22 @@ void COpendriveRoadSegment::update_references(segment_up_ref_t &segment_refs,lan
this->signals[i]->update_references(segment_refs);
for(unsigned int i=0;i<this->objects.size();i++)
this->objects[i]->update_references(segment_refs);
- }
+// }
}
-void COpendriveRoadSegment::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
+void COpendriveRoadSegment::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs,link_up_ref_t &new_link_ref)
{
std::vector<COpendriveRoadSegment *>::iterator seg_it;
std::map<int,COpendriveLane *>::iterator lane_it;
std::vector<COpendriveRoadNode *>::iterator node_it;
- if(this->updated(segment_refs))
- {
+// if(this->updated(segment_refs))
+// {
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
{
if(lane_it->second->updated(lane_refs))
{
- lane_it->second->clean_references(segment_refs,lane_refs,node_refs);
+// lane_it->second->clean_references(segment_refs,lane_refs,node_refs,new_link_ref);
lane_it++;
}
else
@@ -454,7 +460,7 @@ void COpendriveRoadSegment::clean_references(segment_up_ref_t &segment_refs,lane
if(!this->right_neighbor.segment->updated(segment_refs))
this->right_neighbor.segment=NULL;
}
- }
+// }
}
void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
@@ -865,7 +871,7 @@ const COpendriveJunction &COpendriveRoadSegment::get_junction(void) const
throw CException(_HERE_,"Segment does not belong to a junction");
}
-bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment)
+bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment) const
{
for(unsigned int i=0;i<this->connecting.size();i++)
if(this->connecting[i]->get_id()==segment->get_id())
@@ -874,7 +880,7 @@ bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment)
return false;
}
-bool COpendriveRoadSegment::connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2)
+bool COpendriveRoadSegment::connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2) const
{
if(this->connects_to(segment1) && this->connects_to(segment2))
return true;
@@ -889,7 +895,7 @@ void COpendriveRoadSegment::set_junction(COpendriveJunction *junction)
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;
+ TOpendriveWorldPose *start_pose,*end_pose,new_pose;
std::map<int,COpendriveLane *>::iterator lane_it;
lane_up_ref_t::iterator lane_ref_it;
COpendriveLane *new_lane;
@@ -922,7 +928,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,new_link_ref,start_pose,end_pose);
new_lane_ref[lane_it->second]=new_lane;
}
- new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
+// new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
// update geometry
if(start_pose!=NULL)
{
@@ -933,10 +939,10 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
{
geom_it->spline->set_start_point(new_point);
geom_it->spline->generate(this->resolution,geom_it->opendrive->get_max_s()-length);
- start_pose->x=new_point.x;
- start_pose->y=new_point.y;
- start_pose->heading=new_point.heading;
- geom_it->opendrive->set_start_pose(*start_pose);
+ new_pose.x=new_point.x;
+ new_pose.y=new_point.y;
+ new_pose.heading=new_point.heading;
+ geom_it->opendrive->set_start_pose(new_pose);
geom_it->opendrive->set_max_s(geom_it->opendrive->get_max_s()-length);
break;
}
@@ -988,7 +994,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
new_lane=lane_it->second->clone(new_node_ref,new_lane_ref,new_link_ref);
new_lane_ref[lane_it->second]=new_lane;
}
- new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
+// new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
return new_segment;
}
@@ -1080,14 +1086,13 @@ const COpendriveRoadSegment *COpendriveRoadSegment::get_next_path_segment(std::v
cand_segments=this->get_next_segments(input_side,cand_sides);
if(cand_segments.size()==0)
throw CException(_HERE_,"Road too short");
- if(cand_segments.size()==1)
+ for(unsigned int i=0;i<cand_segments.size();i++)
{
try{
- cand_segments[0]->get_node_by_id(*path_it);
- output_side=cand_sides[0];
- return cand_segments[0];
+ cand_segments[i]->get_node_by_id(*path_it);
+ output_side=cand_sides[i];
+ return cand_segments[i];
}catch(CException &e){
- throw CException(_HERE_,"Road segment not in the immediate path");
}
}
if(path_it==path_nodes.end())
@@ -1169,7 +1174,7 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
this->free();
this->set_name(road_info.name().get());
- this->set_id(std::stoi(road_info.id().get()));
+ this->set_id(std::stol(road_info.id().get()));
// only one lane section is supported
if(road_info.lanes().laneSection().size()<1)
{
@@ -1219,7 +1224,7 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
this->objects.push_back(new_object);
}
}
- if(std::stoi(road_info.junction().get())==-1)
+ if(std::stol(road_info.junction().get())==-1)
this->junction_segment=false;
else
this->junction_segment=true;
@@ -1354,39 +1359,45 @@ void COpendriveRoadSegment::save(OpenDRIVE::road_type **road_info)
std::vector<const COpendriveRoadSegment *> segments;
std::vector<int> output_sides;
segments=this->get_next_segments(-1,output_sides);
- if(segments.size()==1 && !segments[0]->is_junction())
+ if(segments.size()>0)
{
- successor.elementType("road");
- successor.elementId(std::to_string(segments[0]->get_id()));
- if(output_sides[0]<0)
- successor.contactPoint("start");
+ if(segments.size()==1 && !segments[0]->is_junction())
+ {
+ successor.elementType("road");
+ successor.elementId(std::to_string(segments[0]->get_id()));
+ if(output_sides[0]<0)
+ successor.contactPoint("start");
+ else
+ successor.contactPoint("end");
+ }
else
+ {
+ successor.elementType("junction");
+ successor.elementId(std::to_string(segments[0]->get_junction().get_id()));
successor.contactPoint("end");
+ }
+ link.successor(successor);
}
- else
- {
- successor.elementType("junction");
- successor.elementId(std::to_string(segments[0]->get_junction().get_id()));
- successor.contactPoint("end");
- }
- link.successor(successor);
segments=this->get_prev_segments(-1,output_sides);
- if(segments.size()==1 && !segments[0]->is_junction())
+ if(segments.size()>0)
{
- predecessor.elementType("road");
- predecessor.elementId(std::to_string(segments[0]->get_id()));
- if(output_sides[0]<0)
- predecessor.contactPoint("end");
+ if(segments.size()==1 && !segments[0]->is_junction())
+ {
+ predecessor.elementType("road");
+ predecessor.elementId(std::to_string(segments[0]->get_id()));
+ if(output_sides[0]<0)
+ predecessor.contactPoint("end");
+ else
+ predecessor.contactPoint("start");
+ }
else
- predecessor.contactPoint("start");
- }
- else
- {
- predecessor.elementType("junction");
- predecessor.elementId(std::to_string(segments[0]->get_junction().get_id()));
- predecessor.contactPoint("end");
+ {
+ predecessor.elementType("junction");
+ predecessor.elementId(std::to_string(segments[0]->get_junction().get_id()));
+ predecessor.contactPoint("end");
+ }
+ link.predecessor(predecessor);
}
- link.predecessor(predecessor);
(*road_info)->lane_link(link);
}
@@ -1564,12 +1575,20 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_right_neighbor(bool &sam
}
}
-double COpendriveRoadSegment::get_length(void) const
+double COpendriveRoadSegment::get_length(bool opendrive) const
{
double length=0.0;
- for(unsigned int i=0;i<this->geometries.size();i++)
- length+=this->geometries[i].opendrive->get_length();
+ if(opendrive)
+ {
+ for(unsigned int i=0;i<this->geometries.size();i++)
+ length+=this->geometries[i].opendrive->get_length();
+ }
+ else
+ {
+ for(unsigned int i=0;i<this->geometries.size();i++)
+ length+=this->geometries[i].spline->get_length();
+ }
return length;
}
@@ -1589,9 +1608,9 @@ TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length,int road_si
for(unsigned int i=0;i<this->geometries.size();i++)
{
- if((int_length-this->geometries[i].opendrive->get_length())<=this->resolution)
+ if((int_length-this->geometries[i].spline->get_length())<=this->resolution)
{
- if (this->geometries[i].spline->evaluate(int_length, spline_pose))
+ if(this->geometries[i].spline->evaluate(int_length, spline_pose))
{
world_pose.x=spline_pose.x;
world_pose.y=spline_pose.y;
@@ -1600,7 +1619,7 @@ TOpendriveWorldPose COpendriveRoadSegment::get_pose_at(double length,int road_si
return world_pose;
}
else
- int_length-=this->geometries[i].opendrive->get_length();
+ int_length-=this->geometries[i].spline->get_length();
}
return world_pose;
@@ -1620,14 +1639,14 @@ double COpendriveRoadSegment::get_curvature_at(double length,int road_side) cons
for(unsigned int i=0;i<this->geometries.size();i++)
{
- if((int_length-this->geometries[i].opendrive->get_length())<=this->resolution)
+ if((int_length-this->geometries[i].spline->get_length())<=this->resolution)
{
if (this->geometries[i].spline->evaluate(int_length, spline_pose))
curvature=spline_pose.curvature;
return curvature;
}
else
- int_length-=this->geometries[i].opendrive->get_length();
+ int_length-=this->geometries[i].spline->get_length();
}
return curvature;
@@ -1842,7 +1861,10 @@ double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendr
length=this->geometries[i].spline->find_closest_point(pose.x,pose.y,closest_spline_point);//,pow(this->resolution,2.0));
if(length<std::numeric_limits<double>::max())
{
- angle=diff_angle(normalize_angle(pose.heading),closest_spline_point.heading);
+// if(this->get_pose_side(pose)>0)
+// angle=diff_angle(normalize_angle(pose.heading),normalize_angle(closest_spline_point.heading+3.14159));
+// else
+ angle=diff_angle(normalize_angle(pose.heading),closest_spline_point.heading);
if(fabs(angle)<angle_tol)
{
dist=sqrt(pow(closest_spline_point.x-pose.x,2.0)+pow(closest_spline_point.y-pose.y,2.0));
@@ -1862,7 +1884,7 @@ double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendr
if(closest_index!=(unsigned int)-1)
{
for(unsigned int i=0;i<closest_index;i++)
- distance+=this->geometries[i].opendrive->get_length();
+ distance+=this->geometries[i].spline->get_length();
}
else
distance=std::numeric_limits<double>::max();