diff --git a/include/junction.h b/include/junction.h index 58ff7efd2392b3d0e9b500a66e95906b1538788f..cc9af97a396a05e3c1d07cefc925dd0a39a3b93c 100644 --- a/include/junction.h +++ b/include/junction.h @@ -40,9 +40,9 @@ class CJunction void link_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road); void unlink_roads_by_index(unsigned int incomming_road,unsigned int outgoing_road); bool are_roads_linked_by_index(unsigned int incomming_road,unsigned int outgoing_road); - CRoadSegment *link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes); - CRoadSegment *link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,unsigned int outgoing_road); - CRoadSegment *link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,TPoint &outgoing_point,unsigned int outgoing_num_lanes); + CRoadSegment *link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes,double outgoing_lane_width); + CRoadSegment *link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,double incomming_lane_width,unsigned int outgoing_road); + CRoadSegment *link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,double incomming_lane_width,TPoint &outgoing_point,unsigned int outgoing_num_lanes,double outgoing_lane_width); void link_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); void unlink_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); bool are_lanes_linked_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); diff --git a/include/road.h b/include/road.h index 43366a24d501165872c9299cafe6d27946eac9a5..7e459dba66f5323ea6f28ec5d48b79ead7d60851 100644 --- a/include/road.h +++ b/include/road.h @@ -70,7 +70,6 @@ class CRoad void remove_segment_by_index(unsigned int index); bool get_point_at(double distance, double lateral_offset,TPoint &point); bool get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max()); - bool get_closest_segment_id(TPoint &point,unsigned int &segment_id,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max()); bool get_segment_id_at(double distance,unsigned int &segment_id); void get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw); diff --git a/include/road_map.h b/include/road_map.h index 589494840f2cc925f9387bcca860820db64972e1..35e35578ae8e30acbe345c9f3355922a9ac69596 100644 --- a/include/road_map.h +++ b/include/road_map.h @@ -82,7 +82,6 @@ class CRoadMap bool get_closest_segment_ids(TPoint &point,std::vector<unsigned int >&segment_ids,std::vector<double >&distances,std::vector<double >&lateral_offsets,double angle_tol=std::numeric_limits<double>::max(),double offset_tol=std::numeric_limits<double>::max()); void get_segment_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw); void get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw); - ~CRoadMap(); }; diff --git a/include/road_segment.h b/include/road_segment.h index 72310e13aed80b3d469d89de93e2fa64109dbf41..7050ab5882768af8bd85e5a23796981a064b8daa 100644 --- a/include/road_segment.h +++ b/include/road_segment.h @@ -18,11 +18,13 @@ class CRoadSegment private: unsigned int id; double resolution; + double lane_width_in; + double lane_width_out; CRoad *parent_road; CJunction *parent_junction; std::vector<CRoadSegment *> prev_segments; std::vector<CRoadSegment *> next_segments; - Eigen::MatrixXd connectivity; + Eigen::MatrixXi connectivity; TPoint start_point; TPoint end_point; CG2Spline spline; @@ -38,7 +40,7 @@ class CRoadSegment bool has_next_segment(CRoadSegment *segment); void remove_next_segment(CRoadSegment *segment); public: - CRoadSegment(unsigned int lanes_in,unsigned int lanes_out); + CRoadSegment(unsigned int lanes_in,double lene_width_in,unsigned int lanes_out,double lane_width_out); unsigned int get_id(void); void set_resolution(double resolution); double get_resolution(void); @@ -52,9 +54,11 @@ class CRoadSegment CRoadSegment &get_next_segment_by_index(unsigned int index); unsigned int get_num_lanes_in(void); unsigned int get_num_lanes_out(void); - double get_lane_width(void); + double get_lane_width_in(void); + double get_lane_width_out(void); + double get_lane_width(double length); double get_lane_speed(void); - unsigned int get_lane(double lateral_offset); + unsigned int get_lane(double lateral_offset,double length); void set_geometry(TPoint &start_point,TPoint &end_point); void get_start_point(TPoint &point); void get_end_point(TPoint &point); @@ -62,12 +66,13 @@ class CRoadSegment void link_lanes(unsigned int lane1,unsigned int lane2); void unlink_lanes(unsigned int lane1,unsigned int lane2); bool are_lanes_linked(unsigned int lane1,unsigned int lane2); - void get_connectivity_matrix(Eigen::MatrixXd &connectivity); + void get_connectivity_matrix(Eigen::MatrixXi &connectivity); bool is_input_lane_connected(unsigned int lane); double get_length(void); bool get_point_at(double distance, double lateral_offset,TPoint &point); bool get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max()); - void get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double lateral_offset=0.0,double start_length=0.0, double end_length=std::numeric_limits<double>::max()); + void get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length=0.0, double end_length=std::numeric_limits<double>::max()); + void get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,unsigned int lane_in=0,unsigned int lane_out=0,double start_length=0.0, double end_length=std::numeric_limits<double>::max()); ~CRoadSegment(); }; diff --git a/src/examples/opendrive_import_test.cpp b/src/examples/opendrive_import_test.cpp index 4e3af3241634f516d235b3442b56c8d379401ced..a7ae2b35e72dcae1d08bd0a5ee2066319edcccba 100644 --- a/src/examples/opendrive_import_test.cpp +++ b/src/examples/opendrive_import_test.cpp @@ -3,19 +3,30 @@ #include <iostream> -//const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr"; -const std::string opendrive_file="/home/sergi/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/build/test_road.xodr"; +const std::string opendrive_file="/home/shernand/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/src/xml/adc_road.xodr"; +//const std::string opendrive_file="/home/shernand/iri-lab/add_robot/add_ws/code/autonomous_driving_tools/build/test_road.xodr"; int main(int argc, char *argv[]) { try{ CRoadMap map,new_map; Eigen::MatrixXi connectivity; - std::vector<unsigned int> id_map,new_path,old_path; + std::vector<unsigned int> new_path,old_path; + std::vector<TLaneID> id_map; std::vector<double> x,y,heading; map.load_opendrive(opendrive_file); + /* + map.get_lane_connectivity(connectivity,id_map); + for(unsigned int i=0;i<id_map.size();i++) + std::cout << i << " -> (" << id_map[i].segment_id << "," << id_map[i].lane_id << ") "; + std::cout << std::endl; + std::cout << "Initial connectivity:" << std::endl; + std::cout << connectivity << std::endl; + + return 0; + */ map.get_lane_geometry(x,y,heading); for(unsigned int i=0;i<x.size();i++) @@ -28,13 +39,6 @@ int main(int argc, char *argv[]) old_path.push_back(22); // old_path.push_back(10); new_path=map.get_path_sub_roadmap(old_path,new_map); - - new_map.get_segment_connectivity(connectivity,id_map); - for(unsigned int i=0;i<id_map.size();i++) - std::cout << i << " -> " << id_map[i] << " "; - std::cout << std::endl; - std::cout << "Initial connectivity:" << std::endl; - std::cout << connectivity << std::endl; } catch (CException &e) { diff --git a/src/examples/osm_import_test.cpp b/src/examples/osm_import_test.cpp index d88250e651355d75c8d39e148eb5e52f7618dea2..c4564b93697905fa1f16b0e4b10ad3be4d30d04e 100644 --- a/src/examples/osm_import_test.cpp +++ b/src/examples/osm_import_test.cpp @@ -5,14 +5,16 @@ //const std::string osm_file="/home/shernand/Downloads/test_osm.osm"; //const std::string osm_file="/home/shernand/Downloads/test_osm2.osm"; -const std::string osm_file="/home/shernand/iri-lab/add_robot/add_ws/src/navigation/OSM/iri_osm_maps/maps/castelldefels.osm"; +const std::string osm_file="/home/shernand/Downloads/diagonal2.osm"; +//const std::string osm_file="/home/shernand/iri-lab/add_robot/add_ws/src/navigation/OSM/iri_osm_maps/maps/castelldefels.osm"; int main(int argc, char *argv[]) { try{ CRoadMap map,new_map; Eigen::MatrixXi connectivity; - std::vector<unsigned int> id_map,new_path,old_path; + std::vector<unsigned int> new_path,old_path; + std::vector<TLaneID> id_map; std::vector<double> x,y,heading; std::vector<TOSMPathData> osm_paths; @@ -21,7 +23,7 @@ int main(int argc, char *argv[]) osm_paths[0].min_turn_radius=DEFAULT_MIN_TURN_RADIUS; osm_paths[0].min_road_length=DEFAULT_MIN_ROAD_LENGTH; osm_paths[0].default_lane_width=4.0; - osm_paths[0].use_default_lane_width=true; + osm_paths[0].use_default_lane_width=false; osm_paths[0].force_two_ways=false; osm_paths[0].highways.push_back("motorway"); osm_paths[0].highways.push_back("trunk"); @@ -53,25 +55,29 @@ int main(int argc, char *argv[]) std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl; return 0; -*/ + */ map.get_lane_geometry(x,y,heading); for(unsigned int i=0;i<x.size();i++) std::cout << x[i] << "," << y[i] << "," << heading[i] << std::endl; return 0; + map.get_lane_connectivity(connectivity,id_map); + return 0; // old_path.push_back(3); // old_path.push_back(4); old_path.push_back(22); // old_path.push_back(10); new_path=map.get_path_sub_roadmap(old_path,new_map); + /* new_map.get_segment_connectivity(connectivity,id_map); for(unsigned int i=0;i<id_map.size();i++) std::cout << i << " -> " << id_map[i] << " "; std::cout << std::endl; std::cout << "Initial connectivity:" << std::endl; std::cout << connectivity << std::endl; + */ } catch (CException &e) { diff --git a/src/junction.cpp b/src/junction.cpp index bc5c9a24c617822fda61ddaf6f0a54ed4161b201..7f54d1329506db6407d4937785b77a9d74a36a79 100644 --- a/src/junction.cpp +++ b/src/junction.cpp @@ -269,7 +269,7 @@ void CJunction::link_roads_by_index(unsigned int incomming_road,unsigned int out out_first_segment=this->outgoing_roads[outgoing_road]->get_first_segment(); in_last_segment->get_end_point(start_point); out_first_segment->get_start_point(end_point); - new_segment=new CRoadSegment(this->incomming_roads[incomming_road]->get_num_lanes(),this->outgoing_roads[outgoing_road]->get_num_lanes()); + new_segment=new CRoadSegment(this->incomming_roads[incomming_road]->get_num_lanes(),this->incomming_roads[incomming_road]->get_lane_width(),this->outgoing_roads[outgoing_road]->get_num_lanes(),this->outgoing_roads[outgoing_road]->get_lane_width()); new_segment->set_resolution(this->resolution); new_segment->set_parent_junction(this); new_segment->set_geometry(start_point,end_point); @@ -349,7 +349,7 @@ bool CJunction::are_roads_linked_by_id(unsigned int incomming_road,unsigned int return this->are_roads_linked_by_index(incomming_index,outgoing_index); } -CRoadSegment *CJunction::link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes) +CRoadSegment *CJunction::link_road_to_point(unsigned int incomming_road,TPoint &outgoing_point,unsigned int outgoing_num_lanes,double outgoing_lane_width) { CRoadSegment *new_segment,*in_last_segment; TPoint start_point,end_point; @@ -363,7 +363,7 @@ CRoadSegment *CJunction::link_road_to_point(unsigned int incomming_road,TPoint & in_last_segment=this->incomming_roads[incomming_index]->get_last_segment(); in_last_segment->get_end_point(start_point); end_point=outgoing_point; - new_segment=new CRoadSegment(this->incomming_roads[incomming_index]->get_num_lanes(),outgoing_num_lanes); + new_segment=new CRoadSegment(this->incomming_roads[incomming_index]->get_num_lanes(),this->incomming_roads[incomming_index]->get_lane_width(),outgoing_num_lanes,outgoing_lane_width); new_segment->set_resolution(this->resolution); new_segment->set_parent_junction(this); new_segment->set_geometry(start_point,end_point); @@ -376,7 +376,7 @@ CRoadSegment *CJunction::link_road_to_point(unsigned int incomming_road,TPoint & return new_segment; } -CRoadSegment *CJunction::link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,unsigned int outgoing_road) +CRoadSegment *CJunction::link_point_to_road(TPoint &incomming_point,unsigned int incomming_num_lanes,double incomming_lane_width,unsigned int outgoing_road) { CRoadSegment *new_segment,*out_first_segment; TPoint start_point,end_point; @@ -390,7 +390,7 @@ CRoadSegment *CJunction::link_point_to_road(TPoint &incomming_point,unsigned int out_first_segment=this->outgoing_roads[outgoing_index]->get_first_segment(); start_point=incomming_point; out_first_segment->get_start_point(end_point); - new_segment=new CRoadSegment(incomming_num_lanes,this->outgoing_roads[outgoing_index]->get_num_lanes()); + new_segment=new CRoadSegment(incomming_num_lanes,incomming_lane_width,this->outgoing_roads[outgoing_index]->get_num_lanes(),this->outgoing_roads[outgoing_index]->get_lane_width()); new_segment->set_resolution(this->resolution); new_segment->set_parent_junction(this); new_segment->set_geometry(start_point,end_point); @@ -403,7 +403,7 @@ CRoadSegment *CJunction::link_point_to_road(TPoint &incomming_point,unsigned int return new_segment; } -CRoadSegment *CJunction::link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,TPoint &outgoing_point,unsigned int outgoing_num_lanes) +CRoadSegment *CJunction::link_point_to_point(TPoint &incomming_point,unsigned int incomming_num_lanes,double incomming_lane_width,TPoint &outgoing_point,unsigned int outgoing_num_lanes,double outgoing_lane_width) { CRoadSegment *new_segment; TPoint start_point,end_point; @@ -412,7 +412,7 @@ CRoadSegment *CJunction::link_point_to_point(TPoint &incomming_point,unsigned in /* create a new road segment */ start_point=incomming_point; end_point=outgoing_point; - new_segment=new CRoadSegment(incomming_num_lanes,outgoing_num_lanes); + new_segment=new CRoadSegment(incomming_num_lanes,incomming_lane_width,outgoing_num_lanes,outgoing_lane_width); new_segment->set_resolution(this->resolution); new_segment->set_parent_junction(this); new_segment->set_geometry(start_point,end_point); @@ -677,7 +677,7 @@ void CJunction::get_segment_geometry(std::vector<double> &x, std::vector<double> yaw.clear(); for(unsigned int i=0;i<this->segments.size();i++) { - this->segments[i]->get_geometry(partial_x,partial_y,partial_heading); + this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,0.0); x.insert(x.end(),partial_x.begin(),partial_x.end()); y.insert(y.end(),partial_y.begin(),partial_y.end()); yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); @@ -687,39 +687,24 @@ void CJunction::get_segment_geometry(std::vector<double> &x, std::vector<double> void CJunction::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw) { std::vector<double> partial_x,partial_y,partial_heading,curvature; - TPoint start_point,end_point; - double lateral_offset_in,lateral_offset_out,width; - CG2Spline *segment_spline; x.clear(); y.clear(); yaw.clear(); for(unsigned int i=0;i<this->segments.size();i++) { - width=this->segments[i]->get_lane_width(); - if(width==0.0) - continue; - - lateral_offset_in=-width/2.0; for(unsigned int j=0;j<this->segments[i]->get_num_lanes_in();j++) { - this->segments[i]->get_point_at(0.0,lateral_offset_in,start_point); - lateral_offset_out=-width/2.0; for(unsigned int k=0;k<this->segments[i]->get_num_lanes_out();k++) { - this->segments[i]->get_point_at(this->segments[i]->get_length(),lateral_offset_out,end_point); if(this->segments[i]->are_lanes_linked(j,k)) { - segment_spline=new CG2Spline(start_point,end_point); - segment_spline->evaluate_all(partial_x,partial_y,curvature,partial_heading); + this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,j,k); x.insert(x.end(),partial_x.begin(),partial_x.end()); y.insert(y.end(),partial_y.begin(),partial_y.end()); yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); - delete segment_spline; } - lateral_offset_out-=width; } - lateral_offset_in-=width; } } } diff --git a/src/opendrive/opendrive_junction.cpp b/src/opendrive/opendrive_junction.cpp index 20fb838f34c1cbd077166558637957b0167023be..dbdee22aeaf8e1b09bdbb0eee4e84d92bf4eec55 100644 --- a/src/opendrive/opendrive_junction.cpp +++ b/src/opendrive/opendrive_junction.cpp @@ -223,12 +223,12 @@ void COpendriveJunction::generate_road_segment(CRoadSegment *segment,unsigned in new_segment->set_center_mark(OD_MARK_NONE); new_segment->set_junction_id(segment->get_parent_junction().get_id()); // add geometry - lateral_offset_in=-(abs(lane_in)-1)*segment->get_lane_width(); + lateral_offset_in=-(abs(lane_in)-1)*segment->get_lane_width_in(); segment->get_point_at(0.0,lateral_offset_in,start_point); start_pose.x=start_point.x; start_pose.y=start_point.y; start_pose.heading=start_point.heading; - lateral_offset_out=-(abs(lane_out)-1)*segment->get_lane_width(); + lateral_offset_out=-(abs(lane_out)-1)*segment->get_lane_width_out(); segment->get_point_at(segment->get_length(),lateral_offset_out,end_point); end_pose.x=end_point.x; end_pose.y=end_point.y; @@ -238,7 +238,7 @@ void COpendriveJunction::generate_road_segment(CRoadSegment *segment,unsigned in // add a single right lane new_lane=new COpendriveLane(); new_lane->set_speed(segment->get_lane_speed()); - new_lane->set_width(segment->get_lane_width()); + new_lane->set_width((segment->get_lane_width_in()+segment->get_lane_width_out())/2.0); new_lane->set_id(-1); new_lane->set_right_mark(OD_MARK_NONE); new_segment->add_right_lane(new_lane); diff --git a/src/road.cpp b/src/road.cpp index c7c1f090c90e352be6962be07a7a1ae616db399a..ee032bfcc9f45e273a1a6ec558ee01c28f4aa73b 100644 --- a/src/road.cpp +++ b/src/road.cpp @@ -269,7 +269,7 @@ void CRoad::add_segment(TPoint &end_point) start_point=this->start_point; else this->segments[this->segments.size()-1]->get_end_point(start_point); - new_segment=new CRoadSegment(this->num_lanes,this->num_lanes); + new_segment=new CRoadSegment(this->num_lanes,this->lane_width,this->num_lanes,this->lane_width); new_segment->set_resolution(this->resolution); new_segment->set_geometry(start_point,end_point); new_segment->set_parent_road(this); @@ -423,7 +423,7 @@ void CRoad::get_segment_geometry(std::vector<double> &x, std::vector<double> &y, yaw.clear(); for(unsigned int i=0;i<this->segments.size();i++) { - this->segments[i]->get_geometry(partial_x,partial_y,partial_heading); + this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,0.0); x.insert(x.end(),partial_x.begin(),partial_x.end()); y.insert(y.end(),partial_y.begin(),partial_y.end()); yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); @@ -433,21 +433,18 @@ void CRoad::get_segment_geometry(std::vector<double> &x, std::vector<double> &y, void CRoad::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw) { std::vector<double> partial_x,partial_y,partial_heading; - double lateral_offset; x.clear(); y.clear(); yaw.clear(); for(unsigned int i=0;i<this->segments.size();i++) { - lateral_offset=-this->lane_width/2.0; for(unsigned int j=0;j<this->num_lanes;j++) { - this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,lateral_offset); + this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,j,j); x.insert(x.end(),partial_x.begin(),partial_x.end()); y.insert(y.end(),partial_y.begin(),partial_y.end()); yaw.insert(yaw.end(),partial_heading.begin(),partial_heading.end()); - lateral_offset-=this->lane_width; } } } diff --git a/src/road_map.cpp b/src/road_map.cpp index 675e9eb9fcc6faec574311e2a640e707499b9fc6..a0b6627c1c669ea1dc5d33de0dc7b476b2a64811 100644 --- a/src/road_map.cpp +++ b/src/road_map.cpp @@ -427,6 +427,7 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in CRoadSegment *new_segment,*old_segment; TPoint start_point,end_point; unsigned int next_id,prev_id,num_lanes_in,num_lanes_out; + double lane_width_in,lane_width_out; std::vector<unsigned int> new_path; new_road_map.free(); @@ -487,6 +488,7 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in prev_id=-1; junction_segments[i]->get_prev_segment_by_index(0).get_end_point(start_point); num_lanes_in=junction_segments[i]->get_prev_segment_by_index(0).get_num_lanes_out(); + lane_width_in=junction_segments[i]->get_prev_segment_by_index(0).get_lane_width_out(); } } else @@ -505,6 +507,7 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in next_id=-1; junction_segments[i]->get_next_segment_by_index(0).get_start_point(end_point); num_lanes_out=junction_segments[i]->get_next_segment_by_index(0).get_num_lanes_in(); + lane_width_out=junction_segments[i]->get_next_segment_by_index(0).get_lane_width_in(); } } else @@ -520,14 +523,14 @@ std::vector<unsigned int> CRoadMap::get_path_sub_roadmap(std::vector<unsigned in if(prev_id==(unsigned int)-1) { if(next_id==(unsigned int)-1) - new_segment=new_junction->link_point_to_point(start_point,num_lanes_in,end_point,num_lanes_out); + new_segment=new_junction->link_point_to_point(start_point,num_lanes_in,lane_width_in,end_point,num_lanes_out,lane_width_out); else - new_segment=new_junction->link_point_to_road(start_point,num_lanes_in,next_id); + new_segment=new_junction->link_point_to_road(start_point,num_lanes_in,lane_width_in,next_id); } else { if(next_id==(unsigned int)-1) - new_segment=new_junction->link_road_to_point(prev_id,end_point,num_lanes_out); + new_segment=new_junction->link_road_to_point(prev_id,end_point,num_lanes_out,lane_width_out); else { new_junction->link_roads_by_id(prev_id,next_id); @@ -758,7 +761,47 @@ void CRoadMap::get_segment_connectivity(Eigen::MatrixXi &connectivity,std::vecto void CRoadMap::get_lane_connectivity(Eigen::MatrixXi &connectivity,std::vector<TLaneID> &id_map) { + unsigned int num_segments=0,max_num_lanes=0,num_lanes_in,num_lanes_out,col_index; + std::vector<CRoadSegment *> segments; + Eigen::MatrixXi block_connectivity; + id_map.clear(); + max_num_lanes=this->get_max_num_lanes(); + for(unsigned int i=0;i<this->roads.size();i++) + { + num_segments+=this->roads[i]->get_num_segments(); + for(unsigned int j=0;j<this->roads[i]->get_num_segments();j++) + segments.push_back(this->roads[i]->segments[j]); + } + for(unsigned int i=0;i<this->junctions.size();i++) + { + num_segments+=this->junctions[i]->get_num_segments(); + for(unsigned int j=0;j<this->junctions[i]->get_num_segments();j++) + segments.push_back(this->junctions[i]->segments[j]); + } + // create the map vector + id_map.resize(num_segments*max_num_lanes); + for(unsigned int i=0;i<num_segments;i++) + for(unsigned int j=0;j<max_num_lanes;j++) + { + id_map[i*max_num_lanes+j].segment_id=segments[i]->get_id(); + id_map[i*max_num_lanes+j].lane_id=j; + } + // create the connectivity matrix + connectivity=Eigen::MatrixXi::Zero(num_segments*max_num_lanes,num_segments*max_num_lanes); + for(unsigned int i=0;i<segments.size();i++) + { + for(unsigned int j=0;j<segments[i]->get_num_next_segments();j++) + { + num_lanes_in=segments[i]->next_segments[j]->get_num_lanes_in(); + num_lanes_out=segments[i]->next_segments[j]->get_num_lanes_out(); + segments[i]->next_segments[j]->get_connectivity_matrix(block_connectivity); + std::cout << num_lanes_in << "," << num_lanes_out << std::endl; + std::cout << block_connectivity << std::endl; + col_index=CRoadSegment::get_index_by_id(segments,segments[i]->next_segments[j]->get_id()); + connectivity.block(i*max_num_lanes,col_index*max_num_lanes,num_lanes_in,num_lanes_out)=block_connectivity; + } + } } unsigned int CRoadMap::get_max_num_lanes(void) @@ -767,9 +810,21 @@ unsigned int CRoadMap::get_max_num_lanes(void) for(unsigned int i=0;i<this->roads.size();i++) { - num_lanes=this->roads[i]->get_num_lanes(); - if(num_lanes>max_num_lanes) - max_num_lanes=num_lanes; + for(unsigned int j=0;j<this->roads[i]->get_num_segments();j++) + { + num_lanes=std::max(this->roads[i]->segments[j]->get_num_lanes_in(),this->roads[i]->segments[j]->get_num_lanes_out()); + if(num_lanes>max_num_lanes) + max_num_lanes=num_lanes; + } + } + for(unsigned int i=0;i<this->junctions.size();i++) + { + for(unsigned int j=0;j<this->junctions[i]->get_num_segments();j++) + { + num_lanes=std::max(this->junctions[i]->segments[j]->get_num_lanes_in(),this->junctions[i]->segments[j]->get_num_lanes_out()); + if(num_lanes>max_num_lanes) + max_num_lanes=num_lanes; + } } return max_num_lanes; diff --git a/src/road_segment.cpp b/src/road_segment.cpp index 3dca6c5f1c16f667809a29b0091f1e797d5d84fd..fc7740024c47156e2b025cb95d33e33f7e20136d 100644 --- a/src/road_segment.cpp +++ b/src/road_segment.cpp @@ -2,15 +2,17 @@ #include "exceptions.h" #include "common.h" -CRoadSegment::CRoadSegment(unsigned int lanes_in,unsigned int lanes_out) +CRoadSegment::CRoadSegment(unsigned int lanes_in,double lane_width_in,unsigned int lanes_out,double lane_width_out) { this->id=-1; this->resolution=0.1; + this->lane_width_in=lane_width_in; + this->lane_width_out=lane_width_out; this->parent_road=NULL; this->parent_junction=NULL; this->prev_segments.clear(); this->next_segments.clear(); - this->connectivity=Eigen::MatrixXd::Zero(lanes_in,lanes_out); + this->connectivity=Eigen::MatrixXi::Zero(lanes_in,lanes_out); this->start_point.x=std::numeric_limits<double>::max(); } @@ -208,35 +210,25 @@ unsigned int CRoadSegment::get_num_lanes_out(void) return this->connectivity.cols(); } -double CRoadSegment::get_lane_width(void) +double CRoadSegment::get_lane_width_in(void) { - double width; + return lane_width_in; +} - if(this->parent_road!=NULL) - return this->parent_road->get_lane_width(); - else - { - if(this->get_num_prev_segments()==1) - { - CRoadSegment &prev_segment=this->get_prev_segment_by_index(0); - if(prev_segment.has_parent_road()) - width=prev_segment.get_parent_road().get_lane_width(); - else - width=4.0; - } - else if(this->get_num_next_segments()==1) - { - CRoadSegment &next_segment=this->get_next_segment_by_index(0); - if(next_segment.has_parent_road()) - width=next_segment.get_parent_road().get_lane_width(); - else - width=4.0; - } - else - width=0.0; - } +double CRoadSegment::get_lane_width_out(void) +{ + return this->lane_width_out; +} + +double CRoadSegment::get_lane_width(double length) +{ + double lane_width; - return width; + if(length<0.0 || length>this->get_length()) + throw CException(_HERE_,"Desired length outside of the road segment "); + lane_width=this->lane_width_in+(this->lane_width_out-this->lane_width_in)*length/this->get_length(); + + return lane_width; } double CRoadSegment::get_lane_speed(void) @@ -244,7 +236,7 @@ double CRoadSegment::get_lane_speed(void) double speed; if(this->parent_road!=NULL) - return this->parent_road->get_lane_width(); + return this->parent_road->get_lane_speed(); else { if(this->get_num_prev_segments()==1) @@ -270,15 +262,13 @@ double CRoadSegment::get_lane_speed(void) return speed; } -unsigned int CRoadSegment::get_lane(double lateral_offset) +unsigned int CRoadSegment::get_lane(double lateral_offset,double length) { unsigned int num_lanes; double lane_width; - if(lateral_offset>0.0) - return 0; + lane_width=this->get_lane_width(length); num_lanes=std::max(this->get_num_lanes_in(),this->get_num_lanes_out()); - lane_width=this->get_lane_width(); for(unsigned int i=0;i<num_lanes;i++) { if(fabs(lateral_offset)<lane_width) @@ -312,7 +302,7 @@ void CRoadSegment::set_full_connectivity(void) { for(unsigned int i=0;i<this->connectivity.rows();i++) for(unsigned int j=0;j<this->connectivity.cols();j++) - this->connectivity(i,j)=1.0; + this->connectivity(i,j)=1; } void CRoadSegment::link_lanes(unsigned int lane1,unsigned int lane2) @@ -321,7 +311,7 @@ void CRoadSegment::link_lanes(unsigned int lane1,unsigned int lane2) throw CException(_HERE_,"Invalid lane1 index"); if(lane2>=this->connectivity.cols()) throw CException(_HERE_,"Invalid lane2 index"); - this->connectivity(lane1,lane2)=1.0; + this->connectivity(lane1,lane2)=1; } void CRoadSegment::unlink_lanes(unsigned int lane1,unsigned int lane2) @@ -330,7 +320,7 @@ void CRoadSegment::unlink_lanes(unsigned int lane1,unsigned int lane2) throw CException(_HERE_,"Invalid lane1 index"); if(lane2>=this->connectivity.cols()) throw CException(_HERE_,"Invalid lane2 index"); - this->connectivity(lane1,lane2)=0.0; + this->connectivity(lane1,lane2)=0; } bool CRoadSegment::are_lanes_linked(unsigned int lane1,unsigned int lane2) @@ -339,13 +329,13 @@ bool CRoadSegment::are_lanes_linked(unsigned int lane1,unsigned int lane2) throw CException(_HERE_,"Invalid lane1 index"); if(lane2>=this->connectivity.cols()) throw CException(_HERE_,"Invalid lane2 index"); - if(this->connectivity(lane1,lane2)==1.0) + if(this->connectivity(lane1,lane2)==1) return true; else return false; } -void CRoadSegment::get_connectivity_matrix(Eigen::MatrixXd &connectivity) +void CRoadSegment::get_connectivity_matrix(Eigen::MatrixXi &connectivity) { connectivity=this->connectivity; } @@ -357,7 +347,7 @@ bool CRoadSegment::is_input_lane_connected(unsigned int lane) if(lane>=this->connectivity.rows()) throw CException(_HERE_,"Invalid lane index"); for(unsigned int i=0;i<this->connectivity.cols();i++) - if(this->connectivity(lane,i)==1.0) + if(this->connectivity(lane,i)==1) num++; if(num>0) return true; @@ -413,51 +403,51 @@ bool CRoadSegment::get_closest_point(TPoint &target_point,TPoint &closest_point, } } -void CRoadSegment::get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double lateral_offset,double start_length, double end_length) +void CRoadSegment::get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length, double end_length) { + CG2Spline *partial_spline; std::vector<double> curvature; - CG2Spline *new_spline=NULL,*partial_spline; - TPoint start_point,end_point; - if(lateral_offset!=0.0) + if(start_length!=0.0 || end_length!=std::numeric_limits<double>::max())// get partial spline { - if(!get_point_at(0.0,lateral_offset,start_point)) - throw CException(_HERE_,"Impossible to compute the new start point"); - if(!get_point_at(this->get_length(),lateral_offset,end_point)) - throw CException(_HERE_,"Impossible to compute the new start point"); - new_spline=new CG2Spline(start_point,end_point); - if(start_length!=0.0 || end_length!=std::numeric_limits<double>::max())// get partial spline + partial_spline=new CG2Spline; + if(!this->spline.get_part(partial_spline,start_length,end_length)) { - partial_spline=new CG2Spline; - if(!new_spline->get_part(partial_spline,start_length,end_length)) - { - delete new_spline; - delete partial_spline; - throw CException(_HERE_,"Impossible to generate partial spline"); - } - partial_spline->evaluate_all(x,y,curvature,yaw); delete partial_spline; + throw CException(_HERE_,"Impossible to generate partial spline"); } - else - new_spline->evaluate_all(x,y,curvature,yaw); - delete new_spline; + partial_spline->evaluate_all(x,y,curvature,yaw); + delete partial_spline; + } + else + this->spline.evaluate_all(x,y,curvature,yaw); +} + +void CRoadSegment::get_geometry(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,unsigned int lane_in,unsigned int lane_out,double start_length, double end_length) +{ + std::vector<double> curvature; + CG2Spline *new_spline=NULL; + TPoint start_point,end_point; + double lateral_offset; + + lateral_offset=-(lane_in+0.5)*this->get_lane_width(start_length); + if(!get_point_at(start_length,lateral_offset,start_point)) + throw CException(_HERE_,"Impossible to compute the new start point"); + if(end_length==std::numeric_limits<double>::max()) + { + lateral_offset=-(lane_out+0.5)*this->get_lane_width(this->get_length()); + if(!get_point_at(this->get_length(),lateral_offset,end_point)) + throw CException(_HERE_,"Impossible to compute the new start point"); } else { - if(start_length!=0.0 || end_length!=std::numeric_limits<double>::max())// get partial spline - { - partial_spline=new CG2Spline; - if(!this->spline.get_part(partial_spline,start_length,end_length)) - { - delete partial_spline; - throw CException(_HERE_,"Impossible to generate partial spline"); - } - partial_spline->evaluate_all(x,y,curvature,yaw); - delete partial_spline; - } - else - this->spline.evaluate_all(x,y,curvature,yaw); + lateral_offset=-(lane_out+0.5)*this->get_lane_width(end_length); + if(!get_point_at(end_length,lateral_offset,end_point)) + throw CException(_HERE_,"Impossible to compute the new start point"); } + new_spline=new CG2Spline(start_point,end_point); + new_spline->evaluate_all(x,y,curvature,yaw); + delete new_spline; } CRoadSegment::~CRoadSegment() diff --git a/src/xml/adc_road.xodr b/src/xml/adc_road.xodr index 2e5deabf9ec3ccd506e31d88fc30b1baab9c1f52..4522a23e0afd0f6cc76e2052fee046ae0429d68c 100644 --- a/src/xml/adc_road.xodr +++ b/src/xml/adc_road.xodr @@ -463,7 +463,7 @@ <left> <lane id="1" type="driving" level="false"> <link /> - <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <width sOffset="0.0000000000000000e+00" a="10.000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> </lane> @@ -477,7 +477,7 @@ <right> <lane id="-1" type="driving" level="false"> <link /> - <width sOffset="0.0000000000000000e+00" a="4.4000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> + <width sOffset="0.0000000000000000e+00" a="10.000000000000004e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00" /> <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="2.9999999999999999e-01" laneChange="both" /> <speed sOffset="0.0000000000000000e+00" max="6.0000000000000000e+01" /> </lane>