From 7abe9b8cd63318ae28c0d78846feae90976d8331 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 7 Sep 2023 17:06:49 +0200
Subject: [PATCH] Solved a bug whne checking the side of the lateral offset.
Added a function to check whether a given input lane is connected to any
output lane.
---
include/road_segment.h | 1 +
src/road_segment.cpp | 17 ++++++++++++++++-
2 files changed, 17 insertions(+), 1 deletion(-)
diff --git a/include/road_segment.h b/include/road_segment.h
index 60c62f3..72310e1 100644
--- a/include/road_segment.h
+++ b/include/road_segment.h
@@ -63,6 +63,7 @@ class CRoadSegment
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);
+ 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());
diff --git a/src/road_segment.cpp b/src/road_segment.cpp
index 33b3b6a..3dca6c5 100644
--- a/src/road_segment.cpp
+++ b/src/road_segment.cpp
@@ -350,6 +350,21 @@ void CRoadSegment::get_connectivity_matrix(Eigen::MatrixXd &connectivity)
connectivity=this->connectivity;
}
+bool CRoadSegment::is_input_lane_connected(unsigned int lane)
+{
+ unsigned int num=0;
+
+ 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)
+ num++;
+ if(num>0)
+ return true;
+ else
+ return false;
+}
+
double CRoadSegment::get_length(void)
{
return this->spline.get_length();
@@ -388,7 +403,7 @@ bool CRoadSegment::get_closest_point(TPoint &target_point,TPoint &closest_point,
{
lateral_offset=sqrt(pow(target_point.x-closest_point.x,2.0)+pow(target_point.y-closest_point.y,2.0));
/* check the side */
- cross=(this->end_point.x-this->start_point.x)*(closest_point.y-this->start_point.y)-(this->end_point.y-this->start_point.y)*(closest_point.x-this->start_point.x);
+ cross=(this->end_point.x-this->start_point.x)*(target_point.y-this->start_point.y)-(this->end_point.y-this->start_point.y)*(target_point.x-this->start_point.x);
if(cross<0.0)
lateral_offset*=-1.0;
return true;
--
GitLab