From b8b34e584f776c0603589c70fe559303ccba1d9b Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 25 Aug 2023 13:11:10 +0200
Subject: [PATCH] Added some helper functions.
---
include/opendrive/opendrive_lane.h | 1 +
include/road_segment.h | 4 +++-
src/road_segment.cpp | 38 +++++++++++++++++++++++++++++-
3 files changed, 41 insertions(+), 2 deletions(-)
diff --git a/include/opendrive/opendrive_lane.h b/include/opendrive/opendrive_lane.h
index 43cddca..959667d 100644
--- a/include/opendrive/opendrive_lane.h
+++ b/include/opendrive/opendrive_lane.h
@@ -10,6 +10,7 @@
class COpendriveLane
{
friend class COpendriveRoadSegment;
+ friend class COpendriveJunction;
private:
opendrive_mark_t right_mark;
double speed;
diff --git a/include/road_segment.h b/include/road_segment.h
index 5164acf..60c62f3 100644
--- a/include/road_segment.h
+++ b/include/road_segment.h
@@ -41,7 +41,7 @@ class CRoadSegment
CRoadSegment(unsigned int lanes_in,unsigned int lanes_out);
unsigned int get_id(void);
void set_resolution(double resolution);
- double get_resolution(double resolution);
+ double get_resolution(void);
bool has_parent_road(void);
CRoad &get_parent_road(void);
bool has_parent_junction(void);
@@ -53,6 +53,7 @@ class CRoadSegment
unsigned int get_num_lanes_in(void);
unsigned int get_num_lanes_out(void);
double get_lane_width(void);
+ double get_lane_speed(void);
unsigned int get_lane(double lateral_offset);
void set_geometry(TPoint &start_point,TPoint &end_point);
void get_start_point(TPoint &point);
@@ -61,6 +62,7 @@ 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);
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());
diff --git a/src/road_segment.cpp b/src/road_segment.cpp
index eab1f7d..33b3b6a 100644
--- a/src/road_segment.cpp
+++ b/src/road_segment.cpp
@@ -140,7 +140,7 @@ void CRoadSegment::set_resolution(double resolution)
this->spline.generate(this->resolution,(unsigned int)10);
}
-double CRoadSegment::get_resolution(double resolution)
+double CRoadSegment::get_resolution(void)
{
return this->resolution;
}
@@ -239,6 +239,37 @@ double CRoadSegment::get_lane_width(void)
return width;
}
+double CRoadSegment::get_lane_speed(void)
+{
+ double speed;
+
+ 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())
+ speed=prev_segment.get_parent_road().get_lane_speed();
+ else
+ speed=50.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())
+ speed=next_segment.get_parent_road().get_lane_speed();
+ else
+ speed=50.0;
+ }
+ else
+ speed=0.0;
+ }
+
+ return speed;
+}
+
unsigned int CRoadSegment::get_lane(double lateral_offset)
{
unsigned int num_lanes;
@@ -314,6 +345,11 @@ bool CRoadSegment::are_lanes_linked(unsigned int lane1,unsigned int lane2)
return false;
}
+void CRoadSegment::get_connectivity_matrix(Eigen::MatrixXd &connectivity)
+{
+ connectivity=this->connectivity;
+}
+
double CRoadSegment::get_length(void)
{
return this->spline.get_length();
--
GitLab