From 7fbce952ce68fe41741a5f0e95f49bffd57442e8 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 10 Mar 2022 09:51:52 +0100
Subject: [PATCH] Solved a bug in the heading cost function: If the trajectory
had less points than the ones reuired to compute the cost, the node crashed.
---
src/ackermann_trajectory_search.cpp | 3 +++
src/heading_cost_function.cpp | 18 ++++++++++++++----
2 files changed, 17 insertions(+), 4 deletions(-)
diff --git a/src/ackermann_trajectory_search.cpp b/src/ackermann_trajectory_search.cpp
index 4989893..ceb10bf 100644
--- a/src/ackermann_trajectory_search.cpp
+++ b/src/ackermann_trajectory_search.cpp
@@ -125,7 +125,10 @@ bool AckermannTrajectorySearch::findBestTrajectory(base_local_planner::Trajector
}
count++;
if (max_samples_ > 0 && count >= max_samples_)
+ {
+ std::cout << "Max sample limit" << std::endl;
break;
+ }
}
if (best_traj_cost >= 0)
{
diff --git a/src/heading_cost_function.cpp b/src/heading_cost_function.cpp
index ad19be0..d7b1846 100644
--- a/src/heading_cost_function.cpp
+++ b/src/heading_cost_function.cpp
@@ -53,10 +53,20 @@ bool HeadingCostFunction::prepare()
double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj)
{
double dist,x,y,theta,near_dist=DBL_MAX,heading_diff=0.0,diff=0.0;
- int interval=traj.getPointsSize()/this->num_points;
+ int interval,samples;
unsigned int near_index=0,i,j;
- for(j=0;j<this->num_points;j++)
+ if(this->num_points>traj.getPointsSize())
+ {
+ interval=1;
+ samples=traj.getPointsSize();
+ }
+ else
+ {
+ interval=traj.getPointsSize()/this->num_points;
+ samples=this->num_points;
+ }
+ for(j=0;j<samples;j++)
{
traj.getPoint((j+1)*interval-1,x,y,theta);
// find the nearrest point on the path
@@ -66,8 +76,8 @@ double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj
(this->global_plan[i].pose.position.y-y)*(this->global_plan[i].pose.position.y-y));
if(dist<near_dist)
{
- near_dist=dist;
- near_index=i;
+ near_dist=dist;
+ near_index=i;
}
}
double v1_x,v1_y;
--
GitLab