customized version of the MapGridCostFunction applied for "goal fro…

…nt" and "alignment" cost functions, updated `RecoveryManager`
rayvburn · Jan 17, 2024 · 824195c · 824195c
1 parent 5df4ce5
commit 824195c
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 9 deletions.
diff --git 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
@@ -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
@@ -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
@@ -61,8 +61,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);