From 9683a4f69e96eb64e2d7640bd2e826003f4a8d52 Mon Sep 17 00:00:00 2001
From: Jarek Karwowski <chromedivizer@gmail.com>
Date: Sat, 6 Jan 2024 02:40:09 +0100
Subject: [PATCH] customized version of the `MapGridCostFunction` applied for
 "goal front" and "alignment" cost functions, updated `RecoveryManager`

---
 CMakeLists.txt                                 | 1 +
 include/humap_local_planner/humap_planner.h    | 9 +++++----
 include/humap_local_planner/recovery_manager.h | 6 +++---
 src/recovery_manager.cpp                       | 4 ++--
 4 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 97a1deea..c322bc37 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -218,6 +218,7 @@ add_library(humap_local_planner
     src/recovery_manager.cpp
     src/path_crossing_detector.cpp
     src/yield_way_crossing_manager.cpp
+    src/map_grid_cost_function.cpp
 )
 add_dependencies(humap_local_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(humap_local_planner
diff --git a/include/humap_local_planner/humap_planner.h b/include/humap_local_planner/humap_planner.h
index ac4a695c..1fb01448 100644
--- a/include/humap_local_planner/humap_planner.h
+++ b/include/humap_local_planner/humap_planner.h
@@ -17,6 +17,7 @@
 #include <humap_local_planner/fformation_space_intrusion_cost_function.h>
 #include <humap_local_planner/passing_speed_cost_function.h>
 #include <humap_local_planner/unsaturated_translation_cost_function.h>
+#include <humap_local_planner/map_grid_cost_function.h>
 
 //for creating a local cost grid
 #include <base_local_planner/map_grid_visualizer.h>
@@ -643,10 +644,10 @@ class HumapPlanner {
 	base_local_planner::MapGridCostFunction path_costs_;
 	/// Cost function that prefers trajectories that go towards (local) goal, based on wave propagation
 	base_local_planner::MapGridCostFunction goal_costs_;
-	/// Cost function that prefers trajectories that keep the robot nose on nose path
-	base_local_planner::MapGridCostFunction alignment_costs_;
-	/// Cost function that prefers trajectories that make the nose go towards (local) nose goal
-	base_local_planner::MapGridCostFunction goal_front_costs_;
+	/// Cost function that prefers trajectories that keep the robot nose on nose path (customized)
+	MapGridCostFunction alignment_costs_;
+	/// Cost function that prefers trajectories that make the nose go towards (local) nose goal (customized)
+	MapGridCostFunction goal_front_costs_;
 	// Penalizes robot for not saturating the trans. vels - moving slower than the maximum speed allows
 	UnsaturatedTranslationCostFunction unsaturated_trans_costs_;
 	/// Cost function that prefers forward trajectories instead of those that consist of backward motions
diff --git a/include/humap_local_planner/recovery_manager.h b/include/humap_local_planner/recovery_manager.h
index f02437fe..31f041c8 100644
--- a/include/humap_local_planner/recovery_manager.h
+++ b/include/humap_local_planner/recovery_manager.h
@@ -1,8 +1,8 @@
 #pragma once
 
-#include <base_local_planner/map_grid_cost_function.h>
 #include <base_local_planner/trajectory.h>
 
+#include <humap_local_planner/map_grid_cost_function.h>
 #include <humap_local_planner/obstacle_separation_cost_function.h>
 
 // custom implementation of the CircularBuffer
@@ -132,8 +132,8 @@ class RecoveryManager {
 	 */
 	void checkDeviation(
 		base_local_planner::Trajectory traj,
-		base_local_planner::MapGridCostFunction& goal_front_costfun,
-		base_local_planner::MapGridCostFunction& alignment_costfun
+		MapGridCostFunction& goal_front_costfun,
+		MapGridCostFunction& alignment_costfun
 	);
 
 	/**
diff --git a/src/recovery_manager.cpp b/src/recovery_manager.cpp
index b2b15275..8fd91870 100644
--- a/src/recovery_manager.cpp
+++ b/src/recovery_manager.cpp
@@ -63,8 +63,8 @@ void RecoveryManager::checkOscillation(
 
 void RecoveryManager::checkDeviation(
 	base_local_planner::Trajectory traj,
-	base_local_planner::MapGridCostFunction& goal_front_costfun,
-	base_local_planner::MapGridCostFunction& alignment_costfun
+	MapGridCostFunction& goal_front_costfun,
+	MapGridCostFunction& alignment_costfun
 ) {
 	double cost_alignment = alignment_costfun.scoreTrajectory(traj);
 	double goal_front_cost = goal_front_costfun.scoreTrajectory(traj);