From 5deccb43eecd878469d3e56d8ee490d4d2379844 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 25 Aug 2023 13:07:49 +0200
Subject: [PATCH] Added functions to automatically generate simple lane
connectivity.
---
include/junction.h | 7 ++++++
src/junction.cpp | 61 +++++++++++++++++++++++++++++++++++++++++++++-
2 files changed, 67 insertions(+), 1 deletion(-)
diff --git a/include/junction.h b/include/junction.h
index d7c7cc2..58ff7ef 100644
--- a/include/junction.h
+++ b/include/junction.h
@@ -16,6 +16,7 @@ class CJunction
friend class CRoad;
friend class CRoadMap;
friend class CRoadSegment;
+ friend class COpendriveJunction;
private:
unsigned int id;
double resolution;
@@ -57,9 +58,11 @@ class CJunction
CRoadMap &get_parent_roadmap(void);
unsigned int get_num_incomming_roads(void);
CRoad &get_incomming_road_by_id(unsigned int id);
+ unsigned int get_incomming_road_index_by_id(unsigned int id);
bool has_incomming_road(unsigned int id);
unsigned int get_num_outgoing_roads(void);
CRoad &get_outgoing_road_by_id(unsigned int id);
+ unsigned int get_outgoing_road_index_by_id(unsigned int id);
bool has_outgoing_road(unsigned int id);
void add_incomming_road(CRoad *road);
void remove_incomming_road_by_id(unsigned int id);
@@ -69,7 +72,11 @@ class CJunction
void unlink_roads_by_id(unsigned int incomming_road,unsigned int outgoing_road);
bool are_roads_linked_by_id(unsigned int incomming_road,unsigned int outgoing_road);
void link_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
+ void link_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road);
+ void link_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road);
void unlink_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
+ void unlink_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road);
+ void unlink_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road);
bool are_lanes_linked_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out);
void get_connectivity_matrix(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &incomming_id_map,std::vector<unsigned int> &outgoing_id_map);
/* geometry */
diff --git a/src/junction.cpp b/src/junction.cpp
index dff89aa..bc5c9a2 100644
--- a/src/junction.cpp
+++ b/src/junction.cpp
@@ -89,6 +89,11 @@ CRoad &CJunction::get_incomming_road_by_id(unsigned int id)
return *this->get_incomming_road_by_index(index);
}
+unsigned int CJunction::get_incomming_road_index_by_id(unsigned int id)
+{
+ return CRoad::get_index_by_id(this->incomming_roads,id);
+}
+
bool CJunction::has_incomming_road(unsigned int id)
{
if(CRoad::get_index_by_id(this->incomming_roads,id)==(unsigned int)-1)
@@ -109,6 +114,11 @@ CRoad *CJunction::get_outgoing_road_by_index(unsigned int index)
return this->outgoing_roads[index];
}
+unsigned int CJunction::get_outgoing_road_index_by_id(unsigned int id)
+{
+ return CRoad::get_index_by_id(this->outgoing_roads,id);
+}
+
CRoad &CJunction::get_outgoing_road_by_id(unsigned int id)
{
unsigned int index;
@@ -413,7 +423,6 @@ CRoadSegment *CJunction::link_point_to_point(TPoint &incomming_point,unsigned in
return new_segment;
}
-
void CJunction::link_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out)
{
CRoadSegment *segment;
@@ -441,6 +450,31 @@ void CJunction::link_lanes_by_id(unsigned int incomming_road,unsigned int lane_i
this->link_lanes_by_index(incomming_index,lane_in,outgoing_index,lane_out);
}
+void CJunction::link_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+ unsigned int num_lanes;
+ unsigned int incomming_index,outgoing_index;
+
+ incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+ outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+
+ num_lanes=std::min(this->incomming_roads[incomming_index]->get_num_lanes(),this->outgoing_roads[outgoing_index]->get_num_lanes());
+ for(unsigned int i=0;i<num_lanes;i++)
+ this->link_lanes_by_index(incomming_index,i,outgoing_index,i);
+}
+
+void CJunction::link_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+ unsigned int incomming_index,outgoing_index;
+
+ incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+ outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+
+ for(unsigned int i=0;i<this->incomming_roads[incomming_index]->get_num_lanes();i++)
+ for(unsigned int j=0;j<this->outgoing_roads[outgoing_index]->get_num_lanes();j++)
+ this->link_lanes_by_index(incomming_index,i,outgoing_index,j);
+}
+
void CJunction::unlink_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out)
{
CRoadSegment *segment;
@@ -468,6 +502,31 @@ void CJunction::unlink_lanes_by_id(unsigned int incomming_road,unsigned int lane
this->unlink_lanes_by_index(incomming_index,lane_in,outgoing_index,lane_out);
}
+void CJunction::unlink_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+ unsigned int num_lanes;
+ unsigned int incomming_index,outgoing_index;
+
+ incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+ outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+
+ num_lanes=std::min(this->incomming_roads[incomming_index]->get_num_lanes(),this->outgoing_roads[outgoing_index]->get_num_lanes());
+ for(unsigned int i=0;i<num_lanes;i++)
+ this->unlink_lanes_by_index(incomming_index,i,outgoing_index,i);
+}
+
+void CJunction::unlink_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road)
+{
+ unsigned int incomming_index,outgoing_index;
+
+ incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road);
+ outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road);
+
+ for(unsigned int i=0;i<this->incomming_roads[incomming_index]->get_num_lanes();i++)
+ for(unsigned int j=0;j<this->outgoing_roads[outgoing_index]->get_num_lanes();j++)
+ this->unlink_lanes_by_index(incomming_index,i,outgoing_index,j);
+}
+
bool CJunction::are_lanes_linked_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out)
{
CRoadSegment *segment;
--
GitLab