diff --git a/include/g2_spline.h b/include/g2_spline.h
index c2faa6c64eb5fc126a8b791f8ec4eb9c0d695a65..11dedc8430a1c7841204b49db7b454f1b06afde6 100644
--- a/include/g2_spline.h
+++ b/include/g2_spline.h
@@ -63,8 +63,8 @@ class CG2Spline
void generate(double resolution,unsigned int iterations=10);
void generate(double resolution,double length,unsigned int iterations=10);
void generate(double &resolution,double n1,double n2, double n3, double n4);
- bool evaluate(double length, TPoint& point);
- void evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading);
+ bool evaluate(double length, TPoint& point,double resolution=DEFAULT_RESOLUTION);
+ void evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading,double resolution=DEFAULT_RESOLUTION);
double get_length(void);
double get_max_curvature(double max_error=0.001,unsigned int max_iter=10);
double get_max_curvature_der(double max_error=0.001,unsigned int max_iter=10);
diff --git a/include/osm/osm_map.h b/include/osm/osm_map.h
index b4251215029efcbb866aa89fa8bef0cc2b5acf57..da1a4511c67e28e0006531946ca13da1c323deec 100644
--- a/include/osm/osm_map.h
+++ b/include/osm/osm_map.h
@@ -46,6 +46,7 @@ class COSMMap : public osmium::handler::Handler
COSMNode *get_node_by_id(long int id);
void add_restriction(COSMRestriction *restriction);
void normalize_locations(std::vector<osmium::Box> &boxes);
+ double get_min_road_length(void);
public:
COSMMap();
void set_utm_zone(int zone);
diff --git a/include/road_segment.h b/include/road_segment.h
index 7050ab5882768af8bd85e5a23796981a064b8daa..981df50d44678d227eed4f75b4c97c9fbff49743 100644
--- a/include/road_segment.h
+++ b/include/road_segment.h
@@ -71,8 +71,10 @@ class CRoadSegment
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 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());
+ void get_segment_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_segment_geometry_from_point(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,TPoint &start_point, double end_length=std::numeric_limits<double>::max());
+ void get_lane_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());
+ void get_lane_geometry_from_point(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,TPoint &start_point,unsigned int lane_out=0,double end_length=std::numeric_limits<double>::max());
~CRoadSegment();
};
diff --git a/src/examples/osm_import_test.cpp b/src/examples/osm_import_test.cpp
index c4564b93697905fa1f16b0e4b10ad3be4d30d04e..33becb27893f68109b822818647ec86fb60af175 100644
--- a/src/examples/osm_import_test.cpp
+++ b/src/examples/osm_import_test.cpp
@@ -5,8 +5,8 @@
//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/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";
+//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[])
{
@@ -18,35 +18,41 @@ int main(int argc, char *argv[])
std::vector<double> x,y,heading;
std::vector<TOSMPathData> osm_paths;
- osm_paths.resize(2);
- osm_paths[0].name="roads";
- osm_paths[0].min_turn_radius=DEFAULT_MIN_TURN_RADIUS;
- osm_paths[0].min_road_length=DEFAULT_MIN_ROAD_LENGTH;
+ osm_paths.resize(3);
+ osm_paths[0].name="road";
+ osm_paths[0].min_turn_radius=5.0;
+ osm_paths[0].min_road_length=0.5;
osm_paths[0].default_lane_width=4.0;
- osm_paths[0].use_default_lane_width=false;
+ osm_paths[0].use_default_lane_width=true;
osm_paths[0].force_two_ways=false;
osm_paths[0].highways.push_back("motorway");
- osm_paths[0].highways.push_back("trunk");
osm_paths[0].highways.push_back("primary");
osm_paths[0].highways.push_back("secondary");
osm_paths[0].highways.push_back("tertiary");
- osm_paths[0].highways.push_back("motor_way_link");
- osm_paths[0].highways.push_back("trunk_link");
- osm_paths[0].highways.push_back("primary_link");
- osm_paths[0].highways.push_back("secondary_link");
- osm_paths[0].highways.push_back("tertiary_link");
- osm_paths[0].highways.push_back("minor");
osm_paths[0].highways.push_back("residential");
- osm_paths[0].highways.push_back("living");
osm_paths[0].highways.push_back("service");
osm_paths[0].highways.push_back("living_street");
+ osm_paths[0].highways.push_back("unclassified");
osm_paths[1].name="pedestrian";
- osm_paths[1].min_turn_radius=DEFAULT_MIN_TURN_RADIUS;
- osm_paths[1].min_road_length=DEFAULT_MIN_ROAD_LENGTH;
+ osm_paths[1].min_turn_radius=5.0;
+ osm_paths[1].min_road_length=0.5;
osm_paths[1].use_default_lane_width=true;
- osm_paths[1].default_lane_width=0.1;
- osm_paths[1].force_two_ways=true;
+ osm_paths[1].default_lane_width=4.0;
+ osm_paths[1].force_two_ways=false;
osm_paths[1].highways.push_back("pedestrian");
+ osm_paths[1].highways.push_back("footway");
+ osm_paths[1].highways.push_back("path");
+ osm_paths[1].highways.push_back("crossing");
+ osm_paths[1].keys.push_back("foot");
+ osm_paths[2].name="bicycle";
+ osm_paths[2].min_turn_radius=5.0;
+ osm_paths[2].min_road_length=0.5;
+ osm_paths[2].use_default_lane_width=true;
+ osm_paths[2].default_lane_width=4.0;
+ osm_paths[2].force_two_ways=false;
+ osm_paths[2].highways.push_back("cycleway");
+ osm_paths[2].keys.push_back("bicycle");
+
map.load_osm(osm_file,osm_paths);
/*
diff --git a/src/g2_spline.cpp b/src/g2_spline.cpp
index 2cd609be84289562c88e05ca1f9b157439572157..3652a51dd5722ac636ed3791af7a771d3f4f530b 100644
--- a/src/g2_spline.cpp
+++ b/src/g2_spline.cpp
@@ -867,10 +867,10 @@ void CG2Spline::generate(double &resolution,double n1,double n2, double n3, doub
this->generated=true;
}
-bool CG2Spline::evaluate(double length, TPoint& point)
+bool CG2Spline::evaluate(double length, TPoint& point,double resolution)
{
if(!this->generated)
- this->generate(DEFAULT_RESOLUTION);
+ this->generate(resolution);
if (length < -this->resolution || length > (this->length[this->num_points-1]+this->resolution))
return false;
else if(length<0.0)
@@ -881,14 +881,14 @@ bool CG2Spline::evaluate(double length, TPoint& point)
return true;
}
-void CG2Spline::evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading)
+void CG2Spline::evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading,double resolution)
{
double pow_u,u;
unsigned int i=0;
double d_x,dd_x,d_y,dd_y;
if(!this->generated)
- this->generate(DEFAULT_RESOLUTION);
+ this->generate(resolution);
x.resize(this->num_points);
y.resize(this->num_points);
curvature.resize(this->num_points);
diff --git a/src/junction.cpp b/src/junction.cpp
index 7f54d1329506db6407d4937785b77a9d74a36a79..8a5ecb3bc5033e05516fa8d487ae0de42c4b6a91 100644
--- a/src/junction.cpp
+++ b/src/junction.cpp
@@ -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,0.0);
+ this->segments[i]->get_segment_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());
@@ -699,7 +699,7 @@ void CJunction::get_lane_geometry(std::vector<double> &x, std::vector<double> &y
{
if(this->segments[i]->are_lanes_linked(j,k))
{
- this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,j,k);
+ this->segments[i]->get_lane_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());
diff --git a/src/osm/osm_junction.cpp b/src/osm/osm_junction.cpp
index b196f15f3c62b31182ea75263566d1d06f76852e..28a5333ca2e217f3b9eac02457a595b3913d1bd6 100644
--- a/src/osm/osm_junction.cpp
+++ b/src/osm/osm_junction.cpp
@@ -679,7 +679,7 @@ void COSMJunction::create_links(void)
in_dist=min_road_length/2.0;
out_dist=(in_radius+out_radius*cos(angle))/fabs(sin(angle));
if(out_dist<min_road_length/2.0)
- out_dist=min_road_length;
+ out_dist=min_road_length/2.0;
}
if(this->add_link(new_link))
{
diff --git a/src/osm/osm_map.cpp b/src/osm/osm_map.cpp
index be755bd4b3c1dfb4d0386f860b9433abeaaee311..d8c7590946eb761e0719b3693b615128b34dba2b 100644
--- a/src/osm/osm_map.cpp
+++ b/src/osm/osm_map.cpp
@@ -234,6 +234,19 @@ void COSMMap::normalize_locations(std::vector<osmium::Box> &boxes)
this->nodes[i]->normalize_location(center_x,center_y);
}
+double COSMMap::get_min_road_length(void)
+{
+ double min_length=std::numeric_limits<double>::max();
+
+ for(unsigned int i=0;i<this->path_types.size();i++)
+ {
+ if(this->path_types[i].data.min_road_length<min_length)
+ min_length=this->path_types[i].data.min_road_length;
+ }
+
+ return min_length;
+}
+
void COSMMap::set_utm_zone(int zone)
{
this->utm_zone=zone;
@@ -439,10 +452,13 @@ void COSMMap::convert(CRoadMap &road)
osm_road_map_t road_map;
osm_map_pair_t map_pair;
std::vector<CRoad *> segment_roads;
+ double resolution;
+ resolution=this->get_min_road_length()/3.0;
+ road.set_resolution(resolution);
for(unsigned int i=0;i<this->segments.size();i++)
{
- this->segments[i]->convert(&left_road,&right_road,road.get_resolution());
+ this->segments[i]->convert(&left_road,&right_road,resolution);
segment_roads.clear();
if(right_road!=NULL)
{
diff --git a/src/road.cpp b/src/road.cpp
index ee032bfcc9f45e273a1a6ec558ee01c28f4aa73b..e870898623c432ce8884b7c770fc8f4ac307c6f4 100644
--- a/src/road.cpp
+++ b/src/road.cpp
@@ -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,0.0);
+ this->segments[i]->get_segment_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());
@@ -441,7 +441,7 @@ void CRoad::get_lane_geometry(std::vector<double> &x, std::vector<double> &y, st
{
for(unsigned int j=0;j<this->num_lanes;j++)
{
- this->segments[i]->get_geometry(partial_x,partial_y,partial_heading,j,j);
+ this->segments[i]->get_lane_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());
diff --git a/src/road_map.cpp b/src/road_map.cpp
index a0b6627c1c669ea1dc5d33de0dc7b476b2a64811..e0db13e334220dbb6d365783b0c813a76141fd1f 100644
--- a/src/road_map.cpp
+++ b/src/road_map.cpp
@@ -796,8 +796,6 @@ void CRoadMap::get_lane_connectivity(Eigen::MatrixXi &connectivity,std::vector<T
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;
}
diff --git a/src/road_segment.cpp b/src/road_segment.cpp
index fc7740024c47156e2b025cb95d33e33f7e20136d..f3c420a36428861fa29cca20effd470ec6152540 100644
--- a/src/road_segment.cpp
+++ b/src/road_segment.cpp
@@ -403,7 +403,7 @@ 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 start_length, double end_length)
+void CRoadSegment::get_segment_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;
@@ -416,14 +416,46 @@ void CRoadSegment::get_geometry(std::vector<double> &x, std::vector<double> &y,
delete partial_spline;
throw CException(_HERE_,"Impossible to generate partial spline");
}
+ partial_spline->generate(this->resolution);
partial_spline->evaluate_all(x,y,curvature,yaw);
delete partial_spline;
}
else
+ {
+ this->spline.generate(this->resolution);
this->spline.evaluate_all(x,y,curvature,yaw);
+ }
+}
+
+void CRoadSegment::get_segment_geometry_from_point(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,TPoint &start_point, double end_length)
+{
+ CG2Spline *partial_spline;
+ std::vector<double> curvature;
+
+ if(end_length!=std::numeric_limits<double>::max())// get partial spline
+ {
+ partial_spline=new CG2Spline;
+ if(!this->spline.get_part(partial_spline,0.0,end_length))
+ {
+ delete partial_spline;
+ throw CException(_HERE_,"Impossible to generate partial spline");
+ }
+ partial_spline->set_start_point(start_point);
+ partial_spline->generate(this->resolution);
+ partial_spline->evaluate_all(x,y,curvature,yaw);
+ delete partial_spline;
+ }
+ else
+ {
+ partial_spline=new CG2Spline(this->spline);
+ partial_spline->set_start_point(start_point);
+ partial_spline->generate(this->resolution);
+ partial_spline->evaluate_all(x,y,curvature,yaw);
+ delete partial_spline;
+ }
}
-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)
+void CRoadSegment::get_lane_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;
@@ -446,6 +478,32 @@ void CRoadSegment::get_geometry(std::vector<double> &x, std::vector<double> &y,
throw CException(_HERE_,"Impossible to compute the new start point");
}
new_spline=new CG2Spline(start_point,end_point);
+ new_spline->generate(this->resolution);
+ new_spline->evaluate_all(x,y,curvature,yaw);
+ delete new_spline;
+}
+
+void CRoadSegment::get_lane_geometry_from_point(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,TPoint &start_point,unsigned int lane_out,double end_length)
+{
+ std::vector<double> curvature;
+ CG2Spline *new_spline=NULL;
+ TPoint end_point;
+ double lateral_offset;
+
+ 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
+ {
+ 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->generate(this->resolution);
new_spline->evaluate_all(x,y,curvature,yaw);
delete new_spline;
}