Add new cost function to encourage higher speeds

base_local_planner/CMakeLists.txt

@@ -94,12 +94,13 @@ add_library(base_local_planner
 	src/align_with_path_function.cpp
 	src/footprint_helper.cpp
 	src/goal_functions.cpp
+	src/latched_stop_rotate_controller.cpp
+	src/local_planner_util.cpp
+	src/magnitude_cost_function.cpp
 	src/map_cell.cpp
 	src/map_grid.cpp
 	src/map_grid_visualizer.cpp
 	src/map_grid_cost_function.cpp
-	src/latched_stop_rotate_controller.cpp
-	src/local_planner_util.cpp
 	src/odometry_helper_ros.cpp
 	src/obstacle_cost_function.cpp
 	src/oscillation_cost_function.cpp

base_local_planner/include/base_local_planner/magnitude_cost_function.h

@@ -0,0 +1,29 @@
+
+#ifndef MAGNITUDE_COST_FUNCTION_H_
+#define MAGNITUDE_COST_FUNCTION_H_
+
+#include <base_local_planner/trajectory_cost_function.h>
+#include <base_local_planner/trajectory.h>
+
+
+namespace base_local_planner {
+
+
+ /**
+  * @brief Gives higher score to trajectories with a higher linear velocity
+ */
+class MagnitudeCostFunction: public base_local_planner::TrajectoryCostFunction {
+public:
+
+  MagnitudeCostFunction();
+
+  ~MagnitudeCostFunction() {}
+
+  double scoreTrajectory(Trajectory &traj);
+
+  bool prepare();
+
+};
+
+} /* namespace base_local_planner */
+#endif /* MAGNITUDE_COST_FUNCTION_H_ */

base_local_planner/src/magnitude_cost_function.cpp

@@ -0,0 +1,21 @@
+#include "base_local_planner/magnitude_cost_function.h"
+#include <cmath>
+
+namespace base_local_planner {
+
+MagnitudeCostFunction::MagnitudeCostFunction()
+{
+
+}
+
+double MagnitudeCostFunction::scoreTrajectory(Trajectory &traj) {
+	double used_v = std::max(0.01, fabs(traj.xv_));
+	return ((1.0 / used_v) * getScale());
+}
+
+bool MagnitudeCostFunction::prepare()
+{
+	return true;
+}
+
+} /* namespace base_local_planner */

dwa_local_planner/cfg/DWAPlanner.cfg

@@ -18,6 +18,7 @@ gen.add("path_distance_bias", double_t, 0, "The weight for the path distance par
 gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 0.8, 0.0)
 gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0)
 gen.add("twirling_scale", double_t, 0, "The weight for penalizing any changes in robot heading", 0.0, 0.0)
+gen.add("velocity_scale", double_t, 0, "The weight for favoring trajectories with higher linear velocity", 0.0, 0.0)
 
 gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0)
 gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0)

dwa_local_planner/include/dwa_local_planner/dwa_planner.h

@@ -60,6 +60,7 @@
 #include <base_local_planner/obstacle_cost_function.h>
 #include <base_local_planner/prefer_forward_cost_function.h>
 #include <base_local_planner/twirling_cost_function.h>
+#include <base_local_planner/magnitude_cost_function.h>
 #include <base_local_planner/simple_scored_sampling_planner.h>
 
 #include <nav_msgs/Path.h>
@@ -189,6 +190,7 @@ namespace dwa_local_planner {
       base_local_planner::MapGridCostFunction goal_front_costs_;
       base_local_planner::MapGridCostFunction alignment_costs_;
       base_local_planner::TwirlingCostFunction twirling_costs_;
+      base_local_planner::MagnitudeCostFunction magnitude_costs_;
       base_local_planner::PreferForwardCostFunction prefer_forward_costs_;
 
       base_local_planner::SimpleScoredSamplingPlanner scored_sampling_planner_;

dwa_local_planner/src/dwa_planner.cpp

@@ -82,6 +82,9 @@ namespace dwa_local_planner {
 
     twirling_costs_.setScale(config.twirling_scale);
 
+    ROS_ERROR_STREAM("magnitude_cost_scale_   " << config.velocity_scale);
+    magnitude_costs_.setScale(config.velocity_scale);
+
     backwardvel_scale_ = 1.2 * config.sim_time * (config.path_distance_bias + config.goal_distance_bias);
     prefer_forward_costs_.setScale(backwardvel_scale_);
 
@@ -165,14 +168,15 @@ namespace dwa_local_planner {
     // (any function returning negative values will abort scoring, so the order can improve performance)
     std::vector<base_local_planner::TrajectoryCostFunction*> critics;
     critics.push_back(&path_align_costs_);
-    critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
+    critics.push_back(&oscillation_costs_); // discards oscillating motions (assigns cost -1)
     critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
     critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
     critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
     critics.push_back(&path_costs_); // prefers trajectories on global path
     critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
     critics.push_back(&prefer_forward_costs_); // prefer trajectories that don't go backwards
     critics.push_back(&twirling_costs_); // optionally prefer trajectories that don't spin
+    critics.push_back(&magnitude_costs_); // optionally prefer trajectories with a higher linear velocity
 
     // trajectory generators
     std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;