Format and add cost_function

Author: sjahr

doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst

@@ -1,4 +1,4 @@
 Using cost functions in the MoveIt Planning Pipeline
 ====================================================
 
-TODO
+TODO

doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp

@@ -8,6 +8,8 @@
 
 #include <moveit_visual_tools/moveit_visual_tools.h>
 
+// Cost functions
+#include <moveit/cost_functions/cost_functions.hpp>
 // Warehouse
 #include <moveit/warehouse/planning_scene_storage.h>
 #include <moveit/warehouse/planning_scene_storage.h>
@@ -183,6 +185,9 @@ class Demo
     // Set start state as current state
     planning_component_->setStartStateToCurrentState();
 
+    // Set cost function
+    planning_component_->setStateCostFunction(moveit::cost_functions::getClearanceCostFn());
+
     auto plan_solution = planning_component_->plan();
 
     // Check if PlanningComponents succeeded in finding the plan