diff --git a/include/common.h b/include/common.h
index 27552d26790b36d8366b4af746258c03c59ba83e..624e88fd32a09eccd5a06faf5abad25a6c82586a 100644
--- a/include/common.h
+++ b/include/common.h
@@ -6,6 +6,9 @@
class COpendriveRoadSegment;
class COSMRoadSegment;
+class CJunction;
+class CRoadSegment;
+class CRoadMap;
class CRoad;
typedef std::pair<COpendriveRoadSegment *,std::vector<CRoad *> > opendrive_map_pair_t;
diff --git a/include/opendrive/opendrive_junction.h b/include/opendrive/opendrive_junction.h
index 4a0a03db92888aeaf1c0486cadb510ada6a6d16f..fce92c53ab74a9ed884f4221dcb6bafeb209b6e1 100644
--- a/include/opendrive/opendrive_junction.h
+++ b/include/opendrive/opendrive_junction.h
@@ -2,6 +2,7 @@
#define _OPENDRIVE_JUNCTION_H
#include "opendrive/opendrive_road_segment.h"
+#include "common.h"
typedef struct{
int from_lane_id;
diff --git a/include/opendrive/opendrive_road_segment.h b/include/opendrive/opendrive_road_segment.h
index f6127f19da9bf0da782a705d81bb192c9bbdd689..04bc1442b4526b25febc1ad4f1eba6b21205c6f4 100644
--- a/include/opendrive/opendrive_road_segment.h
+++ b/include/opendrive/opendrive_road_segment.h
@@ -5,16 +5,13 @@
#include "xml/OpenDRIVE_1.4H.hxx"
#endif
-#include "road.h"
-
#include "opendrive/opendrive_common.h"
#include "opendrive/opendrive_lane.h"
#include "opendrive/opendrive_geometry.h"
#include "opendrive/opendrive_object.h"
#include "opendrive/opendrive_signal.h"
-#include "road_map.h"
-#include "road.h"
+#include "common.h"
typedef struct{
bool road;
diff --git a/include/osm/osm_common.h b/include/osm/osm_common.h
index 843ffcc27e1c43f141539c0016cecb165f24d800..dcbf3643aa5a9de4e3ce1feab4a479c2bc60e59a 100644
--- a/include/osm/osm_common.h
+++ b/include/osm/osm_common.h
@@ -1,9 +1,22 @@
#ifndef _OSM_COMMON_H
#define _OSM_COMMON_H
-#define DEFAULT_LANE_WIDTH 4.0
-#define DEFAULT_MIN_RADIUS 5.0
-#define MIN_ROAD_LENGTH 0.5
+#define DEFAULT_LANE_WIDTH 4.0
+#define DEFAULT_MIN_TURN_RADIUS 5.0
+#define DEFAULT_MIN_ROAD_LENGTH 0.5
+
+#include <string>
+#include <vector>
+
+typedef struct{
+ std::string name;
+ std::vector<std::string> highways;
+ std::vector<std::string> keys;
+ double min_turn_radius;
+ double min_road_length;
+ bool use_default_lane_width;
+ bool force_two_ways;
+}TOSMPathData;
class COSMMap;
class COSMNode;
diff --git a/include/osm/osm_junction.h b/include/osm/osm_junction.h
index 3db8e36ad77ade8400ff927cca3366903d566b73..6004b940e9c87f41090b1f0886e3f81e50a7a208 100644
--- a/include/osm/osm_junction.h
+++ b/include/osm/osm_junction.h
@@ -5,11 +5,11 @@
#include "osm/osm_node.h"
#include "osm/osm_road_segment.h"
+#include "common.h"
+
#include <Eigen/Dense>
#include <vector>
-#include "junction.h"
-
typedef struct{
COSMWay *in_way;
int in_lane_id;
diff --git a/include/osm/osm_map.h b/include/osm/osm_map.h
index 1bf2f79e51230b43ec870a35a97887dd7592747e..c019d18c6c2b3aff1fa356b6fc6ebda210f4480c 100644
--- a/include/osm/osm_map.h
+++ b/include/osm/osm_map.h
@@ -6,12 +6,18 @@
#include "osm/osm_node.h"
#include "osm/osm_restriction.h"
-#include "road_map.h"
-
#include <osmium/handler.hpp>
#include <osmium/io/header.hpp>
#include <osmium/tags/tags_filter.hpp>
+#include "common.h"
+
+typedef struct{
+ TOSMPathData data;
+ osmium::TagsFilter highway_filter;
+ osmium::TagsFilter key_filter;
+}TOSMPathType;
+
class COSMMap : public osmium::handler::Handler
{
friend class COSMWay;
@@ -23,8 +29,8 @@ class COSMMap : public osmium::handler::Handler
std::vector<COSMNode *> nodes;
std::vector<COSMWay *> ways;
std::vector<COSMRestriction *> restrictions;
- osmium::TagsFilter way_filter;
osmium::TagsFilter relation_filter;
+ std::vector<TOSMPathType> path_types;
long int max_id;
int utm_zone;
protected:
@@ -45,6 +51,11 @@ class COSMMap : public osmium::handler::Handler
void set_utm_zone(int zone);
int get_utm_zone(void);
void load(const std::string &filename);
+ bool add_path_type(TOSMPathData &path_data);
+ double get_min_turn_radius(std::string &path_type);
+ double get_min_road_length(std::string &path_type);
+ bool use_default_lane_width(std::string &path_type);
+ bool force_two_ways(std::string &path_type);
void node(const osmium::Node& node);
void way(const osmium::Way& way);
void relation(const osmium::Relation& relation);
diff --git a/include/osm/osm_road_segment.h b/include/osm/osm_road_segment.h
index 14b0205e71b71557eaf214432a05c54717a19568..bee6b8c0cbc052b21b892d74a216ab2b88882125 100644
--- a/include/osm/osm_road_segment.h
+++ b/include/osm/osm_road_segment.h
@@ -5,11 +5,10 @@
#include "osm/osm_way.h"
#include "osm/osm_junction.h"
-#include "road.h"
+#include "common.h"
#include <iostream>
-
class COSMRoadSegment
{
friend class COSMJunction;
diff --git a/include/osm/osm_way.h b/include/osm/osm_way.h
index 3e3bf98904ff9531532a7e7246bac81e1b159c2b..54d928ff1938db20fb70d78c59d4f5910a1b04fe 100644
--- a/include/osm/osm_way.h
+++ b/include/osm/osm_way.h
@@ -15,7 +15,7 @@
#define LAST_SEGMENT -1
#define FIRST_SEGMENT 0
-typedef enum {NO_RESTRICTION=0x00,RESTRICTION_THROUGH=0x01,RESTRICTION_LEFT=0x02,RESTRICTION_RIGHT=0x04} lane_restructions_t;
+typedef enum {NO_RESTRICTION=0x00,RESTRICTION_THROUGH=0x01,RESTRICTION_LEFT=0x02,RESTRICTION_RIGHT=0x04} lane_restrictions_t;
class COSMWay
{
@@ -27,11 +27,12 @@ class COSMWay
private:
long int id;
std::string name;
+ std::string type;
std::vector<COSMNode *> nodes;
unsigned int num_forward_lanes;
- std::vector<lane_restructions_t> forward_lanes;
+ std::vector<lane_restrictions_t> forward_lanes;
unsigned int num_backward_lanes;
- std::vector<lane_restructions_t> backward_lanes;
+ std::vector<lane_restrictions_t> backward_lanes;
double lane_width;
COSMRoadSegment *road_segment;
COSMMap *parent;
@@ -53,14 +54,19 @@ class COSMWay
void clear_restrictions(void);
COSMWay *split(long int node_id);
void merge(COSMWay *way);
- COSMWay(const osmium::Way &way,COSMMap *parent);
+ COSMWay(const osmium::Way &way,COSMMap *parenti,std::string &type);
bool is_not_connected(void);
- bool load_turn_lane_tag(const osmium::Way &way,const std::string &tag,std::vector<lane_restructions_t> &restrictions,unsigned int max_lanes);
+ bool load_turn_lane_tag(const osmium::Way &way,const std::string &tag,std::vector<lane_restrictions_t> &restrictions,unsigned int max_lanes);
void load_lane_restrictions(const osmium::Way &way);
public:
COSMWay(const COSMWay &object);
long int get_id(void);
std::string get_name(void);
+ std::string get_type(void);
+ double get_min_turn_radius(void);
+ double get_min_road_length(void);
+ bool use_default_lane_width(void);
+ bool force_two_ways(void);
unsigned int get_num_nodes(void);
COSMNode &get_node_by_index(unsigned int index);
COSMNode &get_next_node_by_index(unsigned int index);
@@ -79,7 +85,7 @@ class COSMWay
unsigned int get_num_forward_lanes(void);
unsigned int get_num_backward_lanes(void);
bool is_one_way(void);
- lane_restructions_t get_lane_restriction(unsigned int index);
+ lane_restrictions_t get_lane_restriction(unsigned int index);
double get_lane_width(void);
COSMRoadSegment &get_road_segment(void);
COSMMap &get_parent(void);
diff --git a/include/road.h b/include/road.h
index 3af83a6a1a6df723a7744e8b117c3ee22c00a878..43366a24d501165872c9299cafe6d27946eac9a5 100644
--- a/include/road.h
+++ b/include/road.h
@@ -1,18 +1,18 @@
#ifndef _ROAD_H
#define _ROAD_H
-#include "road_map.h"
#include "junction.h"
#include "road_segment.h"
-#include "opendrive/opendrive_road_segment.h"
+
+class CRoadMap;
class CRoad
{
friend class CJunction;
friend class CRoadMap;
friend class CRoadSegment;
- friend class COpendriveRoadSegment;
friend class COSMRoadSegment;
+ friend class COpendriveRoadSegment;
private:
unsigned int id;
double resolution;
diff --git a/include/road_map.h b/include/road_map.h
index b584499974596faf645a4f019d8e4158ca32766b..589494840f2cc925f9387bcca860820db64972e1 100644
--- a/include/road_map.h
+++ b/include/road_map.h
@@ -6,8 +6,7 @@
#include "road_segment.h"
#include "common.h"
-#include "opendrive/opendrive_road_segment.h"
-#include "opendrive/opendrive_junction.h"
+#include "osm/osm_common.h"
#include <Eigen/Dense>
#include <vector>
@@ -53,7 +52,7 @@ class CRoadMap
CRoadMap();
void load_opendrive(const std::string &filename);
void save_opendrive(const std::string &filename);
- void load_osm(const std::string &filename);
+ void load_osm(const std::string &filename,std::vector<TOSMPathData> &path_data);
void set_resolution(double resolution);
double get_resolution(void);
void set_utm_zone(int zone);
diff --git a/src/examples/osm_import_test.cpp b/src/examples/osm_import_test.cpp
index 4d15f586aabc7f31fac924ad24233caf2f9ed2d1..b7ba71a5ae3588e23cdc4ecb001f2dd4fe0e5648 100644
--- a/src/examples/osm_import_test.cpp
+++ b/src/examples/osm_import_test.cpp
@@ -5,7 +5,7 @@
//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";
int main(int argc, char *argv[])
{
@@ -14,8 +14,36 @@ int main(int argc, char *argv[])
Eigen::MatrixXi connectivity;
std::vector<unsigned int> id_map,new_path,old_path;
std::vector<double> x,y,heading;
+ std::vector<TOSMPathData> osm_paths;
- map.load_osm(osm_file);
+ 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[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[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].use_default_lane_width=true;
+ osm_paths[1].force_two_ways=true;
+ osm_paths[1].highways.push_back("pedestrian");
+ map.load_osm(osm_file,osm_paths);
/*
map.get_segment_geometry(x,y,heading);
diff --git a/src/opendrive/opendrive_junction.cpp b/src/opendrive/opendrive_junction.cpp
index 9651b7ffb977807990e060ea354c76b475b0ba53..20fb838f34c1cbd077166558637957b0167023be 100644
--- a/src/opendrive/opendrive_junction.cpp
+++ b/src/opendrive/opendrive_junction.cpp
@@ -1,6 +1,8 @@
#include "opendrive/opendrive_junction.h"
#include "opendrive/opendrive_param_poly3.h"
+#include "junction.h"
+
#include "exceptions.h"
#include <sstream>
diff --git a/src/osm/osm_junction.cpp b/src/osm/osm_junction.cpp
index 9e5ec0f299d54916f32557aa6a5b7a7eaf4f0ff5..b196f15f3c62b31182ea75263566d1d06f76852e 100644
--- a/src/osm/osm_junction.cpp
+++ b/src/osm/osm_junction.cpp
@@ -1,4 +1,5 @@
#include "osm/osm_junction.h"
+#include "junction.h"
#include <math.h>
@@ -507,7 +508,7 @@ void COSMJunction::apply_node_restrictions(void)
void COSMJunction::apply_way_restrictions(void)
{
- lane_restructions_t restriction;
+ lane_restrictions_t restriction;
COSMNode *check_node;
bool left,right;
@@ -585,7 +586,7 @@ void COSMJunction::create_links(void)
{
TOSMLink new_link;
COSMNode *node1,*node3;
- double length1,length2,angle,in_radius,out_radius,in_dist,out_dist;
+ double length1,length2,angle,in_radius,out_radius,in_dist,out_dist,min_road_length,min_radius;
bool left=false,right=false;
for(unsigned int i=0;i<this->connections.size();i++)
@@ -594,6 +595,7 @@ void COSMJunction::create_links(void)
for(unsigned int j=0;j<this->connections[i].size();j++)
{
COSMWay &out_way=this->out_roads[j]->get_parent_way();
+ min_radius=std::min(in_way.get_min_turn_radius(),out_way.get_min_turn_radius());
for(unsigned int k=0;k<in_way.get_num_lanes();k++)
{
for(unsigned int l=0;l<out_way.get_num_lanes();l++)
@@ -604,11 +606,12 @@ void COSMJunction::create_links(void)
new_link.out_way=&out_way;
new_link.in_lane_id=k;
new_link.out_lane_id=l;
+ min_road_length=std::min(in_way.get_min_road_length(),out_way.get_min_road_length());
if(out_way.is_node_first(this->parent_node->get_id()))
node3=&out_way.get_segment_end_node(FIRST_SEGMENT);
else
node3=&out_way.get_segment_start_node(LAST_SEGMENT);
- in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
+ in_radius=min_radius+(in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
if(in_way.is_node_first(this->parent_node->get_id()))
{
node1=&in_way.get_segment_end_node(FIRST_SEGMENT);
@@ -626,34 +629,34 @@ void COSMJunction::create_links(void)
{
node1=&in_way.get_segment_end_node(FIRST_SEGMENT);
if((left=in_way.is_node_left(*node3,false,0.2))==true)
- in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-k)*in_way.get_lane_width();
+ in_radius=min_radius+(in_way.get_num_lanes()/2.0-k)*in_way.get_lane_width();
else if((right=in_way.is_node_right(*node3,false,0.2))==true)
- in_radius=DEFAULT_MIN_RADIUS+(k+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
+ in_radius=min_radius+(k+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
}
else
{
node1=&in_way.get_segment_start_node(LAST_SEGMENT);
if((left=in_way.is_node_left(*node3,true,0.2))==true)
- in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
+ in_radius=min_radius+(in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
else if((right=in_way.is_node_right(*node3,true,0.2))==true)
- in_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-k)*in_way.get_lane_width();
+ in_radius=min_radius+(in_way.get_num_lanes()/2.0-k)*in_way.get_lane_width();
}
*/
- out_radius=DEFAULT_MIN_RADIUS+(out_way.get_num_lanes()/2.0)*out_way.get_lane_width();
+ out_radius=min_radius+(out_way.get_num_lanes()/2.0)*out_way.get_lane_width();
/*
if(out_way.is_node_first(this->parent_node->get_id()))
{
if(left)
- out_radius=DEFAULT_MIN_RADIUS+(l+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
+ out_radius=min_radius+(l+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
else if(right)
- out_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-l)*in_way.get_lane_width();
+ out_radius=min_radius+(in_way.get_num_lanes()/2.0-l)*in_way.get_lane_width();
}
else
{
if(left)
- out_radius=DEFAULT_MIN_RADIUS+(in_way.get_num_lanes()/2.0-l)*in_way.get_lane_width();
+ out_radius=min_radius+(in_way.get_num_lanes()/2.0-l)*in_way.get_lane_width();
else if(right)
- out_radius=DEFAULT_MIN_RADIUS+(l+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
+ out_radius=min_radius+(l+1-in_way.get_num_lanes()/2.0)*in_way.get_lane_width();
}
*/
// std::cout << "in way: " << in_way.id << " lane " << k << " out way: " << out_way.id << " lane " << l << " in radius " << in_radius << " out radius " << out_radius << " num lanes in: " << in_way.get_num_lanes() << " num fwd lanes in: " << in_way.get_num_forward_lanes() << " num lanes out: " << out_way.get_num_lanes() << " num fwd lanes out: " << out_way.get_num_forward_lanes() << std::endl;
@@ -663,20 +666,20 @@ void COSMJunction::create_links(void)
length1=this->parent_node->compute_distance(*node1);
length2=this->parent_node->compute_distance(*node3);
in_dist=std::min(length1,length2)/2.0;
- if(in_dist>DEFAULT_MIN_RADIUS)
- in_dist=DEFAULT_MIN_RADIUS;
- else if(in_dist<MIN_ROAD_LENGTH/2.0)
- in_dist=MIN_ROAD_LENGTH/2.0;
+ if(in_dist>min_radius)
+ in_dist=min_radius;
+ else if(in_dist<min_road_length/2.0)
+ in_dist=min_road_length/2.0;
out_dist=in_dist;
}
else
{
in_dist=(out_radius+in_radius*cos(angle))/fabs(sin(angle));
- if(in_dist<MIN_ROAD_LENGTH/2.0)
- in_dist=MIN_ROAD_LENGTH/2.0;
+ if(in_dist<min_road_length/2.0)
+ 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;
+ if(out_dist<min_road_length/2.0)
+ out_dist=min_road_length;
}
if(this->add_link(new_link))
{
diff --git a/src/osm/osm_map.cpp b/src/osm/osm_map.cpp
index c9d5967e86a8f832da9010e995a427fca13a52b6..ed0872db34b25372bf8fe47e65d29a830642db1b 100644
--- a/src/osm/osm_map.cpp
+++ b/src/osm/osm_map.cpp
@@ -8,17 +8,13 @@
#include <osmium/tags/taglist.hpp>
#include <GeographicLib/UTMUPS.hpp>
-#include "road.h"
-
-const std::vector<std::string> highway_tags={"motorway","trunk","primary","secondary","tertiary","motor_way_link","trunk_link","primary_link","secondary_link","tertiary_link","minor","residential","living","service","living_street"};
+#include "road_map.h"
const std::vector<std::string> relation_type_tags={"restriction"};
+const std::vector<std::string> affirmative_value={"yes","Yes","designated"};
-COSMMap::COSMMap() :
- way_filter{false}
+COSMMap::COSMMap()
{
- for(unsigned int i=0;i<highway_tags.size();i++)
- this->way_filter.add_rule(true, "highway", highway_tags[i]);
for(unsigned int i=0;i<relation_type_tags.size();i++)
this->relation_filter.add_rule(true, "type", relation_type_tags[i]);
this->max_id=-std::numeric_limits<long int>::max();
@@ -280,6 +276,96 @@ void COSMMap::load(const std::string &filename)
this->create_junctions();
}
+bool COSMMap::add_path_type(TOSMPathData &path_data)
+{
+ TOSMPathType new_path_type;
+ bool add;
+
+ for(unsigned int i=0;i<this->path_types.size();i++)
+ if(this->path_types[i].data.name==path_data.name)
+ return false;
+ // add the new path type
+ new_path_type.data.name=path_data.name;
+ new_path_type.data.min_turn_radius=path_data.min_turn_radius;
+ new_path_type.data.min_road_length=path_data.min_road_length;
+ new_path_type.data.use_default_lane_width=path_data.use_default_lane_width;
+ new_path_type.data.force_two_ways=path_data.force_two_ways;
+ for(unsigned int i=0;i<path_data.highways.size();i++)
+ {
+ add=true;
+ for(unsigned int j=0;j<new_path_type.data.highways.size();j++)
+ {
+ if(new_path_type.data.highways[j]==path_data.highways[i])
+ {
+ add=false;
+ break;
+ }
+ }
+ if(add)
+ {
+ new_path_type.highway_filter.add_rule(true, "highway", path_data.highways[i]);
+ new_path_type.data.highways.push_back(path_data.highways[i]);
+ }
+ }
+ for(unsigned int i=0;i<path_data.keys.size();i++)
+ {
+ add=true;
+ for(unsigned int j=0;j<new_path_type.data.keys.size();j++)
+ {
+ if(new_path_type.data.keys[j]==path_data.keys[i])
+ {
+ add=false;
+ break;
+ }
+ }
+ if(add)
+ {
+ for(unsigned int j=0;j<affirmative_value.size();j++)
+ new_path_type.key_filter.add_rule(true, path_data.keys[i],affirmative_value[j]);
+ new_path_type.data.keys.push_back(path_data.keys[i]);
+ }
+ }
+ this->path_types.push_back(new_path_type);
+
+ return true;
+}
+
+double COSMMap::get_min_turn_radius(std::string &path_type)
+{
+ for(unsigned int i=0;i<this->path_types.size();i++)
+ if(this->path_types[i].data.name==path_type)
+ return this->path_types[i].data.min_turn_radius;
+
+ throw CException(_HERE_,"The given path type does not exist");
+}
+
+double COSMMap::get_min_road_length(std::string &path_type)
+{
+ for(unsigned int i=0;i<this->path_types.size();i++)
+ if(this->path_types[i].data.name==path_type)
+ return this->path_types[i].data.min_road_length;
+
+ throw CException(_HERE_,"The given path type does not exist");
+}
+
+bool COSMMap::use_default_lane_width(std::string &path_type)
+{
+ for(unsigned int i=0;i<this->path_types.size();i++)
+ if(this->path_types[i].data.name==path_type)
+ return this->path_types[i].data.use_default_lane_width;
+
+ throw CException(_HERE_,"The given path type does not exist");
+}
+
+bool COSMMap::force_two_ways(std::string &path_type)
+{
+ for(unsigned int i=0;i<this->path_types.size();i++)
+ if(this->path_types[i].data.name==path_type)
+ return this->path_types[i].data.force_two_ways;
+
+ throw CException(_HERE_,"The given path type does not exist");
+}
+
void COSMMap::node(const osmium::Node& node)
{
COSMNode *new_node;
@@ -293,14 +379,29 @@ void COSMMap::way(const osmium::Way& way)
{
COSMWay *new_way=NULL;
- if(osmium::tags::match_any_of(way.tags(),this->way_filter))
+ for(unsigned int i=0;i<this->path_types.size();i++)
{
- try{
- new_way=new COSMWay(way,this);
- this->add_way(new_way);
- }catch(CException &e){
- if(new_way!=NULL)
- delete new_way;
+ if(osmium::tags::match_any_of(way.tags(),this->path_types[i].highway_filter))
+ {
+ try{
+ new_way=new COSMWay(way,this,this->path_types[i].data.name);
+ this->add_way(new_way);
+ }catch(CException &e){
+ if(new_way!=NULL)
+ delete new_way;
+ }
+ return;
+ }
+ if(osmium::tags::match_any_of(way.tags(),this->path_types[i].key_filter))
+ {
+ try{
+ new_way=new COSMWay(way,this,this->path_types[i].data.name);
+ this->add_way(new_way);
+ }catch(CException &e){
+ if(new_way!=NULL)
+ delete new_way;
+ }
+ return;
}
}
}
diff --git a/src/osm/osm_road_segment.cpp b/src/osm/osm_road_segment.cpp
index 7ed8da2e771be666316ba716f513c68d18d02870..c63f039401397aac713cb4a3b010c7b065db9cd6 100644
--- a/src/osm/osm_road_segment.cpp
+++ b/src/osm/osm_road_segment.cpp
@@ -1,5 +1,6 @@
#include "osm/osm_road_segment.h"
#include "exceptions.h"
+#include "road.h"
COSMRoadSegment::COSMRoadSegment(COSMWay *way)
{
@@ -16,7 +17,7 @@ COSMRoadSegment::COSMRoadSegment(COSMWay *way)
void COSMRoadSegment::set_start_distance(double dist)
{
COSMNode *node1,*node2;
- double length,start_end_distance;
+ double length,start_end_distance,min_road_length;
if(dist>this->original_start_distance)
this->original_start_distance=dist;
@@ -24,20 +25,21 @@ void COSMRoadSegment::set_start_distance(double dist)
{
node1=&this->parent_way->get_segment_start_node(FIRST_SEGMENT);
node2=&this->parent_way->get_segment_end_node(FIRST_SEGMENT);
+ min_road_length=this->parent_way->get_min_road_length();
length=node1->compute_distance(*node2);
if(this->parent_way->get_num_nodes()==2)
{
- if((length-this->end_distance-dist)<MIN_ROAD_LENGTH)
+ if((length-this->end_distance-dist)<min_road_length)
{
if(this->original_end_distance==0.0)
- this->start_distance=length-MIN_ROAD_LENGTH;
+ this->start_distance=length-min_road_length;
else
{
start_end_distance=this->original_end_distance+dist;
this->end_distance=this->original_end_distance*length/start_end_distance;
- this->end_distance-=MIN_ROAD_LENGTH/2.0;
+ this->end_distance-=min_road_length/2.0;
this->start_distance=dist*length/start_end_distance;
- this->start_distance-=MIN_ROAD_LENGTH/2.0;
+ this->start_distance-=min_road_length/2.0;
}
}
else
@@ -45,8 +47,8 @@ void COSMRoadSegment::set_start_distance(double dist)
}
else
{
- if((length-dist)<MIN_ROAD_LENGTH)
- this->start_distance=length-MIN_ROAD_LENGTH;
+ if((length-dist)<min_road_length)
+ this->start_distance=length-min_road_length;
else
this->start_distance=dist;
}
@@ -56,7 +58,7 @@ void COSMRoadSegment::set_start_distance(double dist)
void COSMRoadSegment::set_end_distance(double dist)
{
COSMNode *node1,*node2;
- double length,start_end_distance;
+ double length,start_end_distance,min_road_length;
if(dist>this->original_end_distance)
this->original_end_distance=dist;
@@ -64,20 +66,21 @@ void COSMRoadSegment::set_end_distance(double dist)
{
node1=&this->parent_way->get_segment_start_node(LAST_SEGMENT);
node2=&this->parent_way->get_segment_end_node(LAST_SEGMENT);
+ min_road_length=this->parent_way->get_min_road_length();
length=node1->compute_distance(*node2);
if(this->parent_way->get_num_nodes()==2)
{
- if((length-this->start_distance-dist)<MIN_ROAD_LENGTH)
+ if((length-this->start_distance-dist)<min_road_length)
{
if(this->original_start_distance==0.0)
- this->end_distance=length-MIN_ROAD_LENGTH;
+ this->end_distance=length-min_road_length;
else
{
start_end_distance=dist+this->original_start_distance;
this->end_distance=dist*length/start_end_distance;
- this->end_distance-=MIN_ROAD_LENGTH/2.0;
+ this->end_distance-=min_road_length/2.0;
this->start_distance=this->original_start_distance*length/start_end_distance;
- this->start_distance-=MIN_ROAD_LENGTH/2.0;
+ this->start_distance-=min_road_length/2.0;
}
}
else
@@ -85,8 +88,8 @@ void COSMRoadSegment::set_end_distance(double dist)
}
else
{
- if((length-dist)<MIN_ROAD_LENGTH)
- this->end_distance=length-MIN_ROAD_LENGTH;
+ if((length-dist)<min_road_length)
+ this->end_distance=length-min_road_length;
else
this->end_distance=dist;
}
@@ -95,7 +98,7 @@ void COSMRoadSegment::set_end_distance(double dist)
bool COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
{
- double heading1,heading2,x,y,angle,length1,dist,prev_dist,y_offset;
+ double heading1,heading2,x,y,angle,length1,dist,prev_dist,y_offset,min_road_length;
// double angle2,angle3,prev_heading;
TPoint start_point,end_point;
COSMNode *node1,*node2,*node3;
@@ -135,10 +138,11 @@ bool COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
}
else
{
+ min_road_length=this->parent_way->get_min_road_length();
node3=&this->parent_way->get_node_by_index(i+2);
heading2=node2->compute_heading(*node3);
angle=node2->compute_angle(*node1,*node3);
- dist=(DEFAULT_MIN_RADIUS+y_offset)/fabs(tan(angle/2.0));
+ dist=(this->parent_way->get_min_turn_radius()+y_offset)/fabs(tan(angle/2.0));
if(dist>(length1-prev_dist))
{
if(i==0)
@@ -164,8 +168,8 @@ bool COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
}
continue;
}
- else if(dist<MIN_ROAD_LENGTH/2.0)
- dist=MIN_ROAD_LENGTH/2.0;
+ else if(dist<min_road_length/2.0)
+ dist=min_road_length/2.0;
if(length1-prev_dist-dist>resolution)// straight segment
{
end_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
@@ -193,7 +197,7 @@ bool COSMRoadSegment::generate_fwd_geometry(CRoad *road,double resolution)
bool COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
{
- double heading1,heading2,x,y,angle,length1,dist,prev_dist,y_offset;
+ double heading1,heading2,x,y,angle,length1,dist,prev_dist,y_offset,min_road_length;
// double angle2,angle3,prev_heading;
TPoint start_point,end_point;
COSMNode *node1,*node2,*node3;
@@ -234,10 +238,11 @@ bool COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
}
else
{
+ min_road_length=this->parent_way->get_min_road_length();
node3=&this->parent_way->get_node_by_index(i-2);
heading2=node2->compute_heading(*node3);
angle=node2->compute_angle(*node1,*node3);
- dist=(DEFAULT_MIN_RADIUS+y_offset)/fabs(tan(angle/2.0));
+ dist=(this->parent_way->get_min_turn_radius()+y_offset)/fabs(tan(angle/2.0));
if(dist>(length1-prev_dist))
{
if(i==(this->parent_way->get_num_nodes()-1))
@@ -263,8 +268,8 @@ bool COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
}
continue;
}
- else if(dist<MIN_ROAD_LENGTH/2.0)
- dist=MIN_ROAD_LENGTH/2.0;
+ else if(dist<min_road_length/2.0)
+ dist=min_road_length/2.0;
if(length1-prev_dist-dist>resolution)// straight segment
{
end_point.x=x+(length1-dist)*cos(heading1)-y_offset*sin(heading1);
@@ -293,7 +298,7 @@ bool COSMRoadSegment::generate_bwd_geometry(CRoad *road,double resolution)
void COSMRoadSegment::convert(CRoad **left_road,CRoad **right_road,double resolution)
{
double lane_width;
- lane_restructions_t restrictions;
+ lane_restrictions_t restrictions;
CRoadSegment *segment;
unsigned int num_lanes;
diff --git a/src/osm/osm_way.cpp b/src/osm/osm_way.cpp
index e07adfe28af8245eccd017b0eb4b6fe958dc25d8..91cceca41d256ad2924db37b330c6fcd7a95ee1d 100644
--- a/src/osm/osm_way.cpp
+++ b/src/osm/osm_way.cpp
@@ -4,7 +4,7 @@
#include <iostream>
#include <sstream>
-COSMWay::COSMWay(const osmium::Way &way,COSMMap *parent)
+COSMWay::COSMWay(const osmium::Way &way,COSMMap *parent,std::string &type)
{
static unsigned int road_id=0;
std::stringstream new_name;
@@ -23,20 +23,34 @@ COSMWay::COSMWay(const osmium::Way &way,COSMMap *parent)
this->name=new_name.str();
road_id++;
}
+ this->type=type;
// get the number of lanes
- value=way.get_value_by_key("lanes:forward");
- if(value==NULL)
+ if(parent->force_two_ways(type))
{
- value=way.get_value_by_key("lanes");
+ this->num_forward_lanes=1;
+ this->num_backward_lanes=1;
+ }
+ else
+ {
+ value=way.get_value_by_key("lanes:forward");
if(value==NULL)
{
- value=way.get_value_by_key("oneway");
- if(value!=NULL)
+ value=way.get_value_by_key("lanes");
+ if(value==NULL)
{
- if(std::string(value).compare("no")==0)
+ value=way.get_value_by_key("oneway");
+ if(value!=NULL)
{
- this->num_forward_lanes=1;
- this->num_backward_lanes=1;
+ if(std::string(value).compare("no")==0)
+ {
+ this->num_forward_lanes=1;
+ this->num_backward_lanes=1;
+ }
+ else
+ {
+ this->num_forward_lanes=1;
+ this->num_backward_lanes=0;
+ }
}
else
{
@@ -46,22 +60,19 @@ COSMWay::COSMWay(const osmium::Way &way,COSMMap *parent)
}
else
{
- this->num_forward_lanes=1;
- this->num_backward_lanes=0;
- }
- }
- else
- {
- this->num_forward_lanes=std::stoi(std::string(value));
- if(this->num_forward_lanes>1)
- {
- value=way.get_value_by_key("oneway");
- if(value!=NULL)
+ this->num_forward_lanes=std::stoi(std::string(value));
+ if(this->num_forward_lanes>1)
{
- if(std::string(value).compare("no")==0)
+ value=way.get_value_by_key("oneway");
+ if(value!=NULL)
{
- this->num_forward_lanes--;
- this->num_backward_lanes=1;
+ if(std::string(value).compare("no")==0)
+ {
+ this->num_forward_lanes--;
+ this->num_backward_lanes=1;
+ }
+ else
+ this->num_backward_lanes=0;
}
else
this->num_backward_lanes=0;
@@ -69,27 +80,28 @@ COSMWay::COSMWay(const osmium::Way &way,COSMMap *parent)
else
this->num_backward_lanes=0;
}
- else
+ }
+ else
+ {
+ this->num_forward_lanes=std::stoi(std::string(value));
+ value=way.get_value_by_key("lanes:backward");
+ if(value==NULL)
this->num_backward_lanes=0;
-
+ else
+ this->num_backward_lanes=std::stoi(std::string(value));
}
}
+ // get the road width
+ if(parent->use_default_lane_width(type))
+ this->lane_width=DEFAULT_LANE_WIDTH;
else
{
- this->num_forward_lanes=std::stoi(std::string(value));
- value=way.get_value_by_key("lanes:backward");
+ value=way.get_value_by_key("width");
if(value==NULL)
- this->num_backward_lanes=0;
+ this->lane_width=DEFAULT_LANE_WIDTH;
else
- this->num_backward_lanes=std::stoi(std::string(value));
+ this->lane_width=std::stof(std::string(value))/(this->num_forward_lanes+this->num_backward_lanes);
}
- // get the road width
-// value=way.get_value_by_key("width");
-// if(value==NULL)
-// this->lane_width=-1.0;
-// else
-// this->lane_width=std::stof(std::string(value))/(this->num_forward_lanes+this->num_backward_lanes);
- this->lane_width=DEFAULT_LANE_WIDTH;
// get the lane constraints
this->load_lane_restrictions(way);
// create and add the way
@@ -112,6 +124,7 @@ COSMWay::COSMWay(const COSMWay &object)
{
this->id=object.id;
this->name=object.name;
+ this->type=object.type;
this->nodes=object.nodes;
this->num_forward_lanes=object.num_forward_lanes;
this->forward_lanes=object.forward_lanes;
@@ -123,7 +136,7 @@ COSMWay::COSMWay(const COSMWay &object)
this->restrictions=object.restrictions;
}
-bool COSMWay::load_turn_lane_tag(const osmium::Way &way,const std::string &tag,std::vector<lane_restructions_t> &restrictions,unsigned int max_lanes)
+bool COSMWay::load_turn_lane_tag(const osmium::Way &way,const std::string &tag,std::vector<lane_restrictions_t> &restrictions,unsigned int max_lanes)
{
const char *value;
std::size_t prev_pos=0,pos;
@@ -142,9 +155,9 @@ bool COSMWay::load_turn_lane_tag(const osmium::Way &way,const std::string &tag,s
}
way_restrictions.push_back(restriction_text.substr(prev_pos));
if(max_lanes<way_restrictions.size())
- restrictions.resize(max_lanes,(lane_restructions_t)0);
+ restrictions.resize(max_lanes,(lane_restrictions_t)0);
else
- restrictions.resize(way_restrictions.size(),(lane_restructions_t)0);
+ restrictions.resize(way_restrictions.size(),(lane_restrictions_t)0);
for(unsigned int i=0;i<way_restrictions.size() && i<max_lanes;i++)
{
lane_restrictions.clear();
@@ -158,11 +171,11 @@ bool COSMWay::load_turn_lane_tag(const osmium::Way &way,const std::string &tag,s
for(unsigned int j=0;j<lane_restrictions.size();j++)
{
if(lane_restrictions[j].compare("through")==0)
- restrictions[max_lanes-i-1]=(lane_restructions_t)(restrictions[max_lanes-i-1]|RESTRICTION_THROUGH);
+ restrictions[max_lanes-i-1]=(lane_restrictions_t)(restrictions[max_lanes-i-1]|RESTRICTION_THROUGH);
else if(lane_restrictions[j].compare("left")==0)
- restrictions[max_lanes-i-1]=(lane_restructions_t)(restrictions[max_lanes-i-1]|RESTRICTION_LEFT);
+ restrictions[max_lanes-i-1]=(lane_restrictions_t)(restrictions[max_lanes-i-1]|RESTRICTION_LEFT);
else if(lane_restrictions[j].compare("right")==0)
- restrictions[max_lanes-i-1]=(lane_restructions_t)(restrictions[max_lanes-i-1]|RESTRICTION_RIGHT);
+ restrictions[max_lanes-i-1]=(lane_restrictions_t)(restrictions[max_lanes-i-1]|RESTRICTION_RIGHT);
else if(lane_restrictions[j].compare("")==0)
restrictions[max_lanes-i-1]=NO_RESTRICTION;
else
@@ -177,10 +190,10 @@ bool COSMWay::load_turn_lane_tag(const osmium::Way &way,const std::string &tag,s
void COSMWay::load_lane_restrictions(const osmium::Way &way)
{
- std::vector<lane_restructions_t> restrictions;
+ std::vector<lane_restrictions_t> restrictions;
- this->forward_lanes.resize(this->num_forward_lanes,(lane_restructions_t)0);
- this->backward_lanes.resize(this->num_backward_lanes,(lane_restructions_t)0);
+ this->forward_lanes.resize(this->num_forward_lanes,(lane_restrictions_t)0);
+ this->backward_lanes.resize(this->num_backward_lanes,(lane_restrictions_t)0);
if(this->load_turn_lane_tag(way,"turn:lanes",restrictions,this->num_forward_lanes))
this->forward_lanes=restrictions;
@@ -514,6 +527,31 @@ std::string COSMWay::get_name(void)
return this->name;
}
+std::string COSMWay::get_type(void)
+{
+ return this->type;
+}
+
+double COSMWay::get_min_turn_radius(void)
+{
+ return this->parent->get_min_turn_radius(this->type);
+}
+
+double COSMWay::get_min_road_length(void)
+{
+ return this->parent->get_min_road_length(this->type);
+}
+
+bool COSMWay::use_default_lane_width(void)
+{
+ return this->parent->use_default_lane_width(this->type);
+}
+
+bool COSMWay::force_two_ways(void)
+{
+ return this->parent->force_two_ways(this->type);
+}
+
unsigned int COSMWay::get_num_nodes(void)
{
return this->nodes.size();
@@ -753,7 +791,7 @@ unsigned int COSMWay::get_closest_segment(COSMNode &node)
throw CException(_HERE_,"The way has no nodes");
}
-lane_restructions_t COSMWay::get_lane_restriction(unsigned int index)
+lane_restrictions_t COSMWay::get_lane_restriction(unsigned int index)
{
if(index>this->get_num_lanes()-1)
throw CException(_HERE_,"The was does not have so many lanes");
diff --git a/src/road_map.cpp b/src/road_map.cpp
index 81a08efa3dd72883b2b3a1e2f0c2ab02068cddc7..675e9eb9fcc6faec574311e2a640e707499b9fc6 100644
--- a/src/road_map.cpp
+++ b/src/road_map.cpp
@@ -381,10 +381,12 @@ void CRoadMap::save_opendrive(const std::string &filename)
}
}
-void CRoadMap::load_osm(const std::string &filename)
+void CRoadMap::load_osm(const std::string &filename,std::vector<TOSMPathData> &path_data)
{
COSMMap road_map;
road_map.set_utm_zone(this->utm_zone);
+ for(unsigned int i=0;i<path_data.size();i++)
+ road_map.add_path_type(path_data[i]);
this->free();
road_map.load(filename);